From 37bf72e014cec40cc4325985b0c7340bdf2e5ee8 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Wed, 14 Feb 2024 16:38:17 -0500 Subject: [PATCH 01/64] kr_mav_msgs ROS2 compatible --- kr_mav_msgs/CMakeLists.txt | 59 ++++++++++++++++------------- kr_mav_msgs/msg/MotorRPM.msg | 2 +- kr_mav_msgs/msg/OutputData.msg | 2 +- kr_mav_msgs/msg/PWMCommand.msg | 2 +- kr_mav_msgs/msg/PositionCommand.msg | 2 +- kr_mav_msgs/msg/SO3Command.msg | 6 +-- kr_mav_msgs/msg/Serial.msg | 2 +- kr_mav_msgs/msg/StatusData.msg | 2 +- kr_mav_msgs/msg/TRPYCommand.msg | 6 +-- kr_mav_msgs/package.xml | 21 +++++++--- 10 files changed, 60 insertions(+), 44 deletions(-) diff --git a/kr_mav_msgs/CMakeLists.txt b/kr_mav_msgs/CMakeLists.txt index 382ca75f..e7c19daa 100644 --- a/kr_mav_msgs/CMakeLists.txt +++ b/kr_mav_msgs/CMakeLists.txt @@ -1,34 +1,41 @@ -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.8) project(kr_mav_msgs) -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) endif() -find_package(catkin REQUIRED COMPONENTS message_generation geometry_msgs) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +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(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) -add_message_files( - DIRECTORY - msg - FILES - AuxCommand.msg - Corrections.msg - MotorRPM.msg - OutputData.msg - PWMCommand.msg - PositionCommand.msg - SO3Command.msg - Serial.msg - StatusData.msg - TRPYCommand.msg) +find_package(rosidl_default_generators REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} +"msg/AuxCommand.msg" +"msg/Corrections.msg" +"msg/MotorRPM.msg" +"msg/OutputData.msg" +"msg/PositionCommand.msg" +"msg/PWMCommand.msg" +"msg/Serial.msg" +"msg/SO3Command.msg" +"msg/StatusData.msg" +"msg/TRPYCommand.msg" +DEPENDENCIES builtin_interfaces std_msgs geometry_msgs +) -generate_messages(DEPENDENCIES geometry_msgs) +ament_export_dependencies(rosidl_default_runtime) -catkin_package( - CATKIN_DEPENDS - geometry_msgs - message_runtime - DEPENDS - EIGEN3) +ament_package() diff --git a/kr_mav_msgs/msg/MotorRPM.msg b/kr_mav_msgs/msg/MotorRPM.msg index 36593c9c..9f3e2255 100644 --- a/kr_mav_msgs/msg/MotorRPM.msg +++ b/kr_mav_msgs/msg/MotorRPM.msg @@ -1,3 +1,3 @@ -Header header +std_msgs/Header header uint8 motor_count int16[8] rpm diff --git a/kr_mav_msgs/msg/OutputData.msg b/kr_mav_msgs/msg/OutputData.msg index 66eb8d4d..e4cf0222 100644 --- a/kr_mav_msgs/msg/OutputData.msg +++ b/kr_mav_msgs/msg/OutputData.msg @@ -1,4 +1,4 @@ -Header header +std_msgs/Header header uint16 loop_rate float64 voltage geometry_msgs/Quaternion orientation diff --git a/kr_mav_msgs/msg/PWMCommand.msg b/kr_mav_msgs/msg/PWMCommand.msg index e8e1d0a8..ec0631c2 100644 --- a/kr_mav_msgs/msg/PWMCommand.msg +++ b/kr_mav_msgs/msg/PWMCommand.msg @@ -1,3 +1,3 @@ -Header header +std_msgs/Header header # pwm values should be between 0 (1ms pulse width) and 1 (2ms pulse width) float64[2] pwm diff --git a/kr_mav_msgs/msg/PositionCommand.msg b/kr_mav_msgs/msg/PositionCommand.msg index 59625905..80634879 100644 --- a/kr_mav_msgs/msg/PositionCommand.msg +++ b/kr_mav_msgs/msg/PositionCommand.msg @@ -1,4 +1,4 @@ -Header header +std_msgs/Header header geometry_msgs/Point position geometry_msgs/Vector3 velocity geometry_msgs/Vector3 acceleration diff --git a/kr_mav_msgs/msg/SO3Command.msg b/kr_mav_msgs/msg/SO3Command.msg index 015d90ea..61c4cb99 100644 --- a/kr_mav_msgs/msg/SO3Command.msg +++ b/kr_mav_msgs/msg/SO3Command.msg @@ -1,7 +1,7 @@ -Header header +std_msgs/Header header geometry_msgs/Vector3 force geometry_msgs/Quaternion orientation geometry_msgs/Vector3 angular_velocity kr_mav_msgs/AuxCommand aux -float64[3] kR -float64[3] kOm +float64[3] kr +float64[3] kom diff --git a/kr_mav_msgs/msg/Serial.msg b/kr_mav_msgs/msg/Serial.msg index 3e6d466c..bacb338e 100644 --- a/kr_mav_msgs/msg/Serial.msg +++ b/kr_mav_msgs/msg/Serial.msg @@ -6,7 +6,7 @@ uint8 PWM_CMD = 119 # 'w' in base 10 uint8 STATUS_DATA = 99 # 'c' in base 10 uint8 OUTPUT_DATA = 100 # 'd' in base 10 -Header header +std_msgs/Header header uint8 channel uint8 type # One of the types listed above uint8[] data diff --git a/kr_mav_msgs/msg/StatusData.msg b/kr_mav_msgs/msg/StatusData.msg index d176e4f0..2b17fc12 100644 --- a/kr_mav_msgs/msg/StatusData.msg +++ b/kr_mav_msgs/msg/StatusData.msg @@ -1,4 +1,4 @@ -Header header +std_msgs/Header header uint16 loop_rate float64 voltage uint8 seq diff --git a/kr_mav_msgs/msg/TRPYCommand.msg b/kr_mav_msgs/msg/TRPYCommand.msg index f11efb18..1024ecdd 100644 --- a/kr_mav_msgs/msg/TRPYCommand.msg +++ b/kr_mav_msgs/msg/TRPYCommand.msg @@ -1,9 +1,9 @@ -Header header +std_msgs/Header header float64 thrust float64 roll float64 pitch float64 yaw geometry_msgs/Vector3 angular_velocity -float64[3] kR -float64[3] kOm +float64[3] kr +float64[3] kom kr_mav_msgs/AuxCommand aux diff --git a/kr_mav_msgs/package.xml b/kr_mav_msgs/package.xml index 963c645a..160ca018 100644 --- a/kr_mav_msgs/package.xml +++ b/kr_mav_msgs/package.xml @@ -1,19 +1,28 @@ - + kr_mav_msgs 1.0.0 kr_mav_msgs Kartik Mohta Michael Watterson Justin Thomas + Kashish Garg BSD - Kartik Mohta - catkin - + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + std_msgs geometry_msgs - message_generation - message_runtime + action_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + From cef46f665a8b48decf14accf646e425a75567c8b Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Wed, 14 Feb 2024 16:52:56 -0500 Subject: [PATCH 02/64] kr_tracker_msgs ROS2 humble compatible --- trackers/kr_tracker_msgs/CMakeLists.txt | 84 +++++++++++-------- .../action/CircleTracker.action | 8 +- .../kr_tracker_msgs/action/LineTracker.action | 4 +- trackers/kr_tracker_msgs/package.xml | 23 +++-- 4 files changed, 72 insertions(+), 47 deletions(-) diff --git a/trackers/kr_tracker_msgs/CMakeLists.txt b/trackers/kr_tracker_msgs/CMakeLists.txt index 7a0a1902..a4c7c384 100644 --- a/trackers/kr_tracker_msgs/CMakeLists.txt +++ b/trackers/kr_tracker_msgs/CMakeLists.txt @@ -1,38 +1,54 @@ -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.8) project(kr_tracker_msgs) -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) endif() -find_package(catkin REQUIRED COMPONENTS message_generation nav_msgs geometry_msgs actionlib_msgs) - -add_service_files(DIRECTORY srv FILES Transition.srv) - -add_action_files( - DIRECTORY - action - FILES - LineTracker.action - VelocityTracker.action - CircleTracker.action - TrajectoryTracker.action - LissajousTracker.action - LissajousAdder.action) - -add_message_files( - DIRECTORY - msg - FILES - TrackerStatus.msg - VelocityGoal.msg) - -generate_messages(DEPENDENCIES geometry_msgs actionlib_msgs) - -catkin_package( - CATKIN_DEPENDS - nav_msgs - geometry_msgs - actionlib_msgs - message_runtime) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +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(std_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) + +find_package(rosidl_default_generators REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} +"msg/TrackerStatus.msg" +"msg/VelocityGoal.msg" + +"srv/Transition.srv" + +"action/CircleTracker.action" +"action/LineTracker.action" +"action/LissajousAdder.action" +"action/LissajousTracker.action" +"action/TrajectoryTracker.action" +"action/VelocityTracker.action" + +DEPENDENCIES std_msgs builtin_interfaces geometry_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/trackers/kr_tracker_msgs/action/CircleTracker.action b/trackers/kr_tracker_msgs/action/CircleTracker.action index 305a70b7..b6e14b3f 100644 --- a/trackers/kr_tracker_msgs/action/CircleTracker.action +++ b/trackers/kr_tracker_msgs/action/CircleTracker.action @@ -1,7 +1,7 @@ #goal definition -float64 Ax -float64 Ay -float64 T +float64 ax +float64 ay +float64 t float64 duration --- #result definition @@ -12,4 +12,4 @@ float64 length --- #feedback # time in tracker -float64 duration \ No newline at end of file +float64 duration diff --git a/trackers/kr_tracker_msgs/action/LineTracker.action b/trackers/kr_tracker_msgs/action/LineTracker.action index 2b5cb183..eb28824d 100644 --- a/trackers/kr_tracker_msgs/action/LineTracker.action +++ b/trackers/kr_tracker_msgs/action/LineTracker.action @@ -6,8 +6,8 @@ float64 yaw float64 v_des float64 a_des bool relative -time t_start -duration duration +builtin_interfaces/Time t_start +builtin_interfaces/Duration duration --- #result definition # send back goal diff --git a/trackers/kr_tracker_msgs/package.xml b/trackers/kr_tracker_msgs/package.xml index c1269aa7..eee0f91c 100644 --- a/trackers/kr_tracker_msgs/package.xml +++ b/trackers/kr_tracker_msgs/package.xml @@ -1,20 +1,29 @@ - + kr_tracker_msgs 1.0.0 kr_tracker_msgs Dinesh Thakur Kartik Mohta + Kashish Garg BSD - Dinesh Thakur - catkin + ament_cmake - nav_msgs + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + action_msgs + std_msgs + builtin_interfaces geometry_msgs - actionlib_msgs - message_generation - message_runtime + ament_lint_auto + ament_lint_common + + + ament_cmake + From d8b9dfb82dbe1fd3765b2bdcf1fe2854ca2d5810 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Thu, 15 Feb 2024 14:30:19 -0500 Subject: [PATCH 03/64] cmake version 3.8->3.10 --- kr_mav_msgs/CMakeLists.txt | 2 +- trackers/kr_tracker_msgs/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/kr_mav_msgs/CMakeLists.txt b/kr_mav_msgs/CMakeLists.txt index e7c19daa..2d9062bc 100644 --- a/kr_mav_msgs/CMakeLists.txt +++ b/kr_mav_msgs/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.10) project(kr_mav_msgs) # Default to C99 diff --git a/trackers/kr_tracker_msgs/CMakeLists.txt b/trackers/kr_tracker_msgs/CMakeLists.txt index a4c7c384..d8eb6bdf 100644 --- a/trackers/kr_tracker_msgs/CMakeLists.txt +++ b/trackers/kr_tracker_msgs/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.10) project(kr_tracker_msgs) # Default to C99 From 4f8cfd91d26d9719d3e39c4eb4335a864045b0a2 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Fri, 16 Feb 2024 15:52:28 -0500 Subject: [PATCH 04/64] mesh visualization in ROS2 humble --- kr_mesh_visualization/CMakeLists.txt | 55 ++++--- kr_mesh_visualization/README.md | 40 +++++ kr_mesh_visualization/launch/test.launch | 17 -- kr_mesh_visualization/launch/test_launch.py | 39 +++++ kr_mesh_visualization/package.xml | 19 ++- .../src/mesh_visualization.cpp | 151 ++++++++++-------- 6 files changed, 206 insertions(+), 115 deletions(-) create mode 100644 kr_mesh_visualization/README.md delete mode 100644 kr_mesh_visualization/launch/test.launch create mode 100644 kr_mesh_visualization/launch/test_launch.py diff --git a/kr_mesh_visualization/CMakeLists.txt b/kr_mesh_visualization/CMakeLists.txt index b17fdb4c..b165539b 100644 --- a/kr_mesh_visualization/CMakeLists.txt +++ b/kr_mesh_visualization/CMakeLists.txt @@ -1,34 +1,41 @@ cmake_minimum_required(VERSION 3.10) project(kr_mesh_visualization) -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) -find_package(catkin REQUIRED COMPONENTS geometry_msgs nav_msgs topic_tools visualization_msgs) +add_executable(${PROJECT_NAME} src/mesh_visualization.cpp) +ament_target_dependencies(${PROJECT_NAME} rclcpp geometry_msgs nav_msgs visualization_msgs) -catkin_package( - CATKIN_DEPENDS - geometry_msgs - nav_msgs - topic_tools - visualization_msgs) +install(DIRECTORY + mesh + launch + DESTINATION share/${PROJECT_NAME} +) -add_executable(${PROJECT_NAME} src/mesh_visualization.cpp) -target_include_directories(${PROJECT_NAME} PRIVATE ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PRIVATE ${catkin_LIBRARIES}) +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} +) -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +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() -install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) -install(DIRECTORY mesh/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/mesh) +ament_package() \ No newline at end of file diff --git a/kr_mesh_visualization/README.md b/kr_mesh_visualization/README.md new file mode 100644 index 00000000..e0951cc9 --- /dev/null +++ b/kr_mesh_visualization/README.md @@ -0,0 +1,40 @@ +kr_mesh_visualization +============= + +Package to visualize the drone in RViz. + +## Message types supported: + +Three message types are supported in this package in order to subscribe to the pose data and visualize the drone. The default message type that is being subscribed to is `nav_msgs/msg/Odometry`. The other message types supported are `geometry_msgs/msg/PoseStamped` and `geometry_msgs/msg/PoseWithCovarianceStamped`. The subscribers for the other two message types have been commented out since ROS2 Humble does not allow multiple subscribers to subscribe to a topic with different message types. + +If you need to use another a particular message type, please uncomment the respective subscriber and comment out the rest. + +## Testing kr_mesh_visualization + +To run a demo of the package: + +1. You can either run package directly using `ros2 run` or using the launch file. + + a. First method: use the following command + ``` + ros2 run kr_mesh_visualization kr_mesh_visualization + ``` + + b: Second method: use the following command + ``` + ros2 launch kr_mesh_visualization test_launch.py + ``` + +2. Launch RViz using the command `rviz2`. Add the `Marker` topic to `Displays`. + +3. Publish a demo message to see the mesh visualization. The command is + + ``` + ros2 topic pub /topic_name nav_msgs/msg/Odometry '{header: {stamp: {sec: 0, nanosec: 0}, frame_id: "map"}, child_frame_id: "", pose: {pose: {position: {x: 0.0, y: 0.0, z: 1.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}, twist: {twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}}' + ``` + + Based on the method used to run the package, the `/topic_name` might change. + + a. For first method `/topic_name` is `/mesh_visualization/input` + + b. For the second method `/topic_name` is `/quadrotor/odom` \ No newline at end of file diff --git a/kr_mesh_visualization/launch/test.launch b/kr_mesh_visualization/launch/test.launch deleted file mode 100644 index 62ce480b..00000000 --- a/kr_mesh_visualization/launch/test.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - diff --git a/kr_mesh_visualization/launch/test_launch.py b/kr_mesh_visualization/launch/test_launch.py new file mode 100644 index 00000000..ae40a7e8 --- /dev/null +++ b/kr_mesh_visualization/launch/test_launch.py @@ -0,0 +1,39 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import Node + +def generate_launch_description(): + + input_topic_launch_arg = DeclareLaunchArgument( + "~/input", + default_value=TextSubstitution(text="/quadrotor/odom") + ) + + new_frame_id_launch_arg = DeclareLaunchArgument( + "new_frame_id", default_value=TextSubstitution(text="") + ) + + mesh_visualization_node = Node( + package="kr_mesh_visualization", + executable="kr_mesh_visualization", + name="mesh_visualization", + output="screen", + remappings=[ + ("~/input", LaunchConfiguration("~/input")) + ], + parameters=[ + {"mesh_resource": "package://kr_mesh_visualization/mesh/hummingbird.mesh"}, + {"color/r": 0.0}, + {"color/g": 0.0}, + {"color/b": 1.0}, + {"color/a": 0.7}, + {"new_frame_id": LaunchConfiguration("new_frame_id")} + ] + ) + + return LaunchDescription([ + input_topic_launch_arg, + new_frame_id_launch_arg, + mesh_visualization_node + ]) \ No newline at end of file diff --git a/kr_mesh_visualization/package.xml b/kr_mesh_visualization/package.xml index cacbdfce..0cd712bf 100644 --- a/kr_mesh_visualization/package.xml +++ b/kr_mesh_visualization/package.xml @@ -1,20 +1,25 @@ - + kr_mesh_visualization 1.0.0 kr_mesh_visualization - + Kartik Mohta Michael Watterson - + Pranav Shah BSD - Kartik Mohta - catkin + ament_cmake - nav_msgs + rclcpp geometry_msgs - topic_tools + nav_msgs visualization_msgs + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/kr_mesh_visualization/src/mesh_visualization.cpp b/kr_mesh_visualization/src/mesh_visualization.cpp index 7701cb77..8a0ac20a 100644 --- a/kr_mesh_visualization/src/mesh_visualization.cpp +++ b/kr_mesh_visualization/src/mesh_visualization.cpp @@ -1,83 +1,100 @@ -#include -#include -#include -#include -#include -#include +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "visualization_msgs/msg/marker.hpp" -static std::string mesh_resource, new_frame_id; -static ros::Publisher pub_vis; -static double color_r, color_g, color_b, color_a; -static double scale_x, scale_y, scale_z; - -static void publishMarker(const std::string &frame_id, const geometry_msgs::Pose &pose) +class MeshVisualizationNode : public rclcpp::Node { - visualization_msgs::Marker marker; - marker.header.frame_id = (new_frame_id == "") ? frame_id : new_frame_id; - marker.header.stamp = ros::Time(); // time 0 so that the marker will be - // displayed regardless of the current time - marker.ns = ros::this_node::getName(); - marker.id = 0; - marker.type = visualization_msgs::Marker::MESH_RESOURCE; - marker.action = visualization_msgs::Marker::ADD; - marker.pose = pose; - marker.scale.x = scale_x; - marker.scale.y = scale_y; - marker.scale.z = scale_z; - marker.color.a = color_a; - marker.color.r = color_r; - marker.color.g = color_g; - marker.color.b = color_b; - marker.mesh_resource = mesh_resource; - pub_vis.publish(marker); -} + public: + MeshVisualizationNode() : Node("kr_mesh_visualization") + { + this->declare_parameter("mesh_resource", + std::string("package://kr_mesh_visualization/mesh/hummingbird.mesh")); + this->declare_parameter("color/r", 1.0); + this->declare_parameter("color/g", 0.0); + this->declare_parameter("color/b", 0.0); + this->declare_parameter("color/a", 1.0); + this->declare_parameter("scale/x", 1.0); + this->declare_parameter("scale/y", 1.0); + this->declare_parameter("scale/z", 1.0); + this->declare_parameter("new_frame_id", std::string("")); -static void any_callback(const topic_tools::ShapeShifter::ConstPtr &msg) -{ - // ROS_INFO_STREAM("Type: " << msg->getDataType()); + this->get_parameter("mesh_resource", mesh_resource); + this->get_parameter("color/r", color_r); + this->get_parameter("color/g", color_g); + this->get_parameter("color/b", color_b); + this->get_parameter("color/a", color_a); + this->get_parameter("scale/x", scale_x); + this->get_parameter("scale/y", scale_y); + this->get_parameter("scale/z", scale_z); + this->get_parameter("new_frame_id", new_frame_id); - if(msg->getDataType() == "geometry_msgs/PoseStamped") + using namespace std::placeholders; + pub_vis_ = this->create_publisher("~/robot", 10); + sub_odom_ = this->create_subscription("~/input", 10, + std::bind(&MeshVisualizationNode::odom_callback, this, _1)); + + + /* Uncomment any of the below subscribers to support another message type */ + // sub_pwcs_ = this->create_subscription( + // "~/input", 10, std::bind(&MeshVisualizationNode::callback_pose_with_covariance_stamped, this, _1)); + // sub_ps_ = this->create_subscription( + // "~/input", 10, std::bind(&MeshVisualizationNode::callback_pose_stamped, this, _1)); + } + + protected: + std::string mesh_resource, new_frame_id; + rclcpp::Publisher::SharedPtr pub_vis_; + rclcpp::Subscription::SharedPtr sub_ps_; + rclcpp::Subscription::SharedPtr sub_pwcs_; + rclcpp::Subscription::SharedPtr sub_odom_; + double color_r, color_g, color_b, color_a; + double scale_x, scale_y, scale_z; + + void publishMarker(const std::string &frame_id, const geometry_msgs::msg::Pose &pose) { - auto pose_msg = msg->instantiate(); - publishMarker(pose_msg->header.frame_id, pose_msg->pose); + auto marker = visualization_msgs::msg::Marker(); + + marker.header.frame_id = (new_frame_id == "") ? frame_id : new_frame_id; + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = this->get_name(); + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = pose; + marker.scale.x = scale_x; + marker.scale.y = scale_y; + marker.scale.z = scale_z; + marker.color.a = color_a; + marker.color.r = color_r; + marker.color.g = color_g; + marker.color.b = color_b; + marker.mesh_resource = mesh_resource; + pub_vis_->publish(marker); } - else if(msg->getDataType() == "geometry_msgs/PoseWithCovarianceStamped") + + void callback_pose_stamped(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - auto pose_msg = msg->instantiate(); - publishMarker(pose_msg->header.frame_id, pose_msg->pose.pose); + publishMarker(msg->header.frame_id, msg->pose); } - else if(msg->getDataType() == "nav_msgs/Odometry") + + void callback_pose_with_covariance_stamped(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - auto odom_msg = msg->instantiate(); - publishMarker(odom_msg->header.frame_id, odom_msg->pose.pose); + publishMarker(msg->header.frame_id, msg->pose.pose); } - else + + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { - ROS_ERROR_STREAM(ros::this_node::getName() << " got unsupported message type " << msg->getDataType()); + publishMarker(msg->header.frame_id, msg->pose.pose); } -} +}; int main(int argc, char **argv) { - ros::init(argc, argv, "kr_mesh_visualization"); - - ros::NodeHandle nh("~"); - - nh.param("mesh_resource", mesh_resource, std::string("package://kr_mesh_visualization/mesh/hummingbird.mesh")); - nh.param("color/r", color_r, 1.0); - nh.param("color/g", color_g, 0.0); - nh.param("color/b", color_b, 0.0); - nh.param("color/a", color_a, 1.0); - nh.param("scale/x", scale_x, 1.0); - nh.param("scale/y", scale_y, 1.0); - nh.param("scale/z", scale_z, 1.0); - - nh.param("new_frame_id", new_frame_id, std::string("")); - - pub_vis = nh.advertise("robot", 10); - - ros::Subscriber any_sub = nh.subscribe("input", 10, &any_callback, ros::TransportHints().tcpNoDelay()); - ros::spin(); - + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); return 0; -} +} \ No newline at end of file From 90d2eabe5d43de2c562b8108b66c3b22b18599df Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Fri, 16 Feb 2024 21:28:35 -0500 Subject: [PATCH 05/64] parameter for message type --- kr_mesh_visualization/launch/test_launch.py | 11 +++++-- .../src/mesh_visualization.cpp | 31 ++++++++++++++----- 2 files changed, 32 insertions(+), 10 deletions(-) diff --git a/kr_mesh_visualization/launch/test_launch.py b/kr_mesh_visualization/launch/test_launch.py index ae40a7e8..c7061931 100644 --- a/kr_mesh_visualization/launch/test_launch.py +++ b/kr_mesh_visualization/launch/test_launch.py @@ -14,6 +14,11 @@ def generate_launch_description(): "new_frame_id", default_value=TextSubstitution(text="") ) + msg_type_launch_arg = DeclareLaunchArgument( + "msg_type", + default_value=TextSubstitution(text="nav_msgs/msg/Odometry") + ) + mesh_visualization_node = Node( package="kr_mesh_visualization", executable="kr_mesh_visualization", @@ -28,12 +33,14 @@ def generate_launch_description(): {"color/g": 0.0}, {"color/b": 1.0}, {"color/a": 0.7}, - {"new_frame_id": LaunchConfiguration("new_frame_id")} + {"new_frame_id": LaunchConfiguration("new_frame_id")}, + {"msg_type": LaunchConfiguration("msg_type")} ] ) return LaunchDescription([ input_topic_launch_arg, new_frame_id_launch_arg, + msg_type_launch_arg, mesh_visualization_node - ]) \ No newline at end of file + ]) diff --git a/kr_mesh_visualization/src/mesh_visualization.cpp b/kr_mesh_visualization/src/mesh_visualization.cpp index 8a0ac20a..56ebd23b 100644 --- a/kr_mesh_visualization/src/mesh_visualization.cpp +++ b/kr_mesh_visualization/src/mesh_visualization.cpp @@ -19,6 +19,7 @@ class MeshVisualizationNode : public rclcpp::Node this->declare_parameter("scale/y", 1.0); this->declare_parameter("scale/z", 1.0); this->declare_parameter("new_frame_id", std::string("")); + this->declare_parameter("msg_type", std::string("nav_msgs/msg/Odometry")); this->get_parameter("mesh_resource", mesh_resource); this->get_parameter("color/r", color_r); @@ -29,18 +30,31 @@ class MeshVisualizationNode : public rclcpp::Node this->get_parameter("scale/y", scale_y); this->get_parameter("scale/z", scale_z); this->get_parameter("new_frame_id", new_frame_id); + this->get_parameter("msg_type", msg_type); using namespace std::placeholders; pub_vis_ = this->create_publisher("~/robot", 10); - sub_odom_ = this->create_subscription("~/input", 10, - std::bind(&MeshVisualizationNode::odom_callback, this, _1)); + if(msg_type == "nav_msgs/msg/Odometry") + { + sub_odom_ = this->create_subscription("~/input", 10, + std::bind(&MeshVisualizationNode::odom_callback, this, _1)); + } + else if(msg_type == "geometry_msgs/msg/PoseStamped") + { + sub_ps_ = this->create_subscription( + "~/input", 10, std::bind(&MeshVisualizationNode::callback_pose_stamped, this, _1)); + } + else if(msg_type == "geometry_msgs/msg/PoseWithCovarianceStamped") + { + sub_pwcs_ = this->create_subscription( + "~/input", 10, std::bind(&MeshVisualizationNode::callback_pose_with_covariance_stamped, this, _1)); + } + else + { + RCLCPP_ERROR_STREAM(this->get_logger(), std::string(this->get_name()) << " got unsupported message type " << msg_type << "\n"); + } - /* Uncomment any of the below subscribers to support another message type */ - // sub_pwcs_ = this->create_subscription( - // "~/input", 10, std::bind(&MeshVisualizationNode::callback_pose_with_covariance_stamped, this, _1)); - // sub_ps_ = this->create_subscription( - // "~/input", 10, std::bind(&MeshVisualizationNode::callback_pose_stamped, this, _1)); } protected: @@ -51,6 +65,7 @@ class MeshVisualizationNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_odom_; double color_r, color_g, color_b, color_a; double scale_x, scale_y, scale_z; + std::string msg_type; void publishMarker(const std::string &frame_id, const geometry_msgs::msg::Pose &pose) { @@ -97,4 +112,4 @@ int main(int argc, char **argv) rclcpp::spin(node); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} From 97a724c665b08571f62c4f539511284d913b21d2 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Fri, 16 Feb 2024 21:32:11 -0500 Subject: [PATCH 06/64] format edit --- kr_mesh_visualization/CMakeLists.txt | 2 +- kr_mesh_visualization/README.md | 34 ++----------------- .../src/mesh_visualization.cpp | 7 ++-- 3 files changed, 6 insertions(+), 37 deletions(-) diff --git a/kr_mesh_visualization/CMakeLists.txt b/kr_mesh_visualization/CMakeLists.txt index b165539b..0bb9f529 100644 --- a/kr_mesh_visualization/CMakeLists.txt +++ b/kr_mesh_visualization/CMakeLists.txt @@ -38,4 +38,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_package() \ No newline at end of file +ament_package() diff --git a/kr_mesh_visualization/README.md b/kr_mesh_visualization/README.md index e0951cc9..16c10753 100644 --- a/kr_mesh_visualization/README.md +++ b/kr_mesh_visualization/README.md @@ -5,36 +5,6 @@ Package to visualize the drone in RViz. ## Message types supported: -Three message types are supported in this package in order to subscribe to the pose data and visualize the drone. The default message type that is being subscribed to is `nav_msgs/msg/Odometry`. The other message types supported are `geometry_msgs/msg/PoseStamped` and `geometry_msgs/msg/PoseWithCovarianceStamped`. The subscribers for the other two message types have been commented out since ROS2 Humble does not allow multiple subscribers to subscribe to a topic with different message types. +Three message types are supported in this package in order to subscribe to the pose data and visualize the drone. The default message type that is being subscribed to is `nav_msgs/msg/Odometry`. The other message types supported are `geometry_msgs/msg/PoseStamped` and `geometry_msgs/msg/PoseWithCovarianceStamped`. ROS2 does not support multiple subscribers to the same topic with different message type. -If you need to use another a particular message type, please uncomment the respective subscriber and comment out the rest. - -## Testing kr_mesh_visualization - -To run a demo of the package: - -1. You can either run package directly using `ros2 run` or using the launch file. - - a. First method: use the following command - ``` - ros2 run kr_mesh_visualization kr_mesh_visualization - ``` - - b: Second method: use the following command - ``` - ros2 launch kr_mesh_visualization test_launch.py - ``` - -2. Launch RViz using the command `rviz2`. Add the `Marker` topic to `Displays`. - -3. Publish a demo message to see the mesh visualization. The command is - - ``` - ros2 topic pub /topic_name nav_msgs/msg/Odometry '{header: {stamp: {sec: 0, nanosec: 0}, frame_id: "map"}, child_frame_id: "", pose: {pose: {position: {x: 0.0, y: 0.0, z: 1.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}, twist: {twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}}' - ``` - - Based on the method used to run the package, the `/topic_name` might change. - - a. For first method `/topic_name` is `/mesh_visualization/input` - - b. For the second method `/topic_name` is `/quadrotor/odom` \ No newline at end of file +In order to change the message type for the subscriber, you can use the parameter `msg_type` to change the message type to any one of the 3 supported types. This parameter accepts a string of the message types listed above. diff --git a/kr_mesh_visualization/src/mesh_visualization.cpp b/kr_mesh_visualization/src/mesh_visualization.cpp index 56ebd23b..3c00c8c0 100644 --- a/kr_mesh_visualization/src/mesh_visualization.cpp +++ b/kr_mesh_visualization/src/mesh_visualization.cpp @@ -34,11 +34,11 @@ class MeshVisualizationNode : public rclcpp::Node using namespace std::placeholders; pub_vis_ = this->create_publisher("~/robot", 10); - + if(msg_type == "nav_msgs/msg/Odometry") { - sub_odom_ = this->create_subscription("~/input", 10, - std::bind(&MeshVisualizationNode::odom_callback, this, _1)); + sub_odom_ = this->create_subscription( + "~/input", 10, std::bind(&MeshVisualizationNode::odom_callback, this, _1)); } else if(msg_type == "geometry_msgs/msg/PoseStamped") { @@ -54,7 +54,6 @@ class MeshVisualizationNode : public rclcpp::Node { RCLCPP_ERROR_STREAM(this->get_logger(), std::string(this->get_name()) << " got unsupported message type " << msg_type << "\n"); } - } protected: From cc30c147285133a87c62d3d39eb4cccb07b94ea3 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Wed, 13 Mar 2024 18:18:28 -0400 Subject: [PATCH 07/64] kr_mav_controllers ROS2 humble compatible --- kr_mav_controllers/CMakeLists.txt | 113 ++- kr_mav_controllers/README.md | 14 + kr_mav_controllers/cfg/SO3.cfg | 51 -- .../include/kr_mav_controllers/PIDControl.hpp | 8 +- .../{SO3Control.h => SO3Control.hpp} | 8 +- .../kr_mav_controllers/so3_control_tester.hpp | 100 +-- kr_mav_controllers/nodelet_plugin.xml | 23 - kr_mav_controllers/package.xml | 25 +- kr_mav_controllers/src/PIDControl.cpp | 3 +- kr_mav_controllers/src/SO3Control.cpp | 9 +- .../src/pid_control_component.cpp | 184 +++++ .../src/pid_control_nodelet.cpp | 173 ----- .../src/so3_control_component.cpp | 649 +++++++++++++++++ .../src/so3_control_nodelet.cpp | 376 ---------- kr_mav_controllers/src/so3_trpy_control.cpp | 660 ++++++++++++------ .../launch/so3_control_component_test.test.py | 91 +++ .../test/so3_control_component_test.cpp | 241 +++++++ .../test/so3_control_nodelet.test | 12 - .../test/so3_control_nodelet_test.cpp | 230 ------ 19 files changed, 1755 insertions(+), 1215 deletions(-) create mode 100644 kr_mav_controllers/README.md delete mode 100755 kr_mav_controllers/cfg/SO3.cfg rename kr_mav_controllers/include/kr_mav_controllers/{SO3Control.h => SO3Control.hpp} (93%) delete mode 100644 kr_mav_controllers/nodelet_plugin.xml create mode 100644 kr_mav_controllers/src/pid_control_component.cpp delete mode 100644 kr_mav_controllers/src/pid_control_nodelet.cpp create mode 100644 kr_mav_controllers/src/so3_control_component.cpp delete mode 100644 kr_mav_controllers/src/so3_control_nodelet.cpp create mode 100755 kr_mav_controllers/test/launch/so3_control_component_test.test.py create mode 100644 kr_mav_controllers/test/so3_control_component_test.cpp delete mode 100644 kr_mav_controllers/test/so3_control_nodelet.test delete mode 100644 kr_mav_controllers/test/so3_control_nodelet_test.cpp diff --git a/kr_mav_controllers/CMakeLists.txt b/kr_mav_controllers/CMakeLists.txt index 0a150fbb..7e105f62 100644 --- a/kr_mav_controllers/CMakeLists.txt +++ b/kr_mav_controllers/CMakeLists.txt @@ -1,83 +1,58 @@ cmake_minimum_required(VERSION 3.10) project(kr_mav_controllers) -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) - -find_package(catkin REQUIRED COMPONENTS - dynamic_reconfigure - geometry_msgs - kr_mav_msgs - nav_msgs - nodelet - roscpp - std_msgs - tf -) -find_package(Eigen3 REQUIRED NO_MODULE) - -generate_dynamic_reconfigure_options(cfg/SO3.cfg) - -catkin_package( - INCLUDE_DIRS - LIBRARIES - CATKIN_DEPENDS - dynamic_reconfigure - geometry_msgs - kr_mav_msgs - nav_msgs - nodelet - roscpp - std_msgs - tf - DEPENDS - EIGEN3 -) - -add_library(kr_mav_so3_control src/SO3Control.cpp src/so3_control_nodelet.cpp src/so3_trpy_control.cpp) -target_include_directories(kr_mav_so3_control PUBLIC include ${catkin_INCLUDE_DIRS}) -target_link_libraries(kr_mav_so3_control PUBLIC ${catkin_LIBRARIES} Eigen3::Eigen) -add_dependencies(kr_mav_so3_control ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) - -# TODO: PID control has not been updated for new messages add_library(kr_mav_pid_control src/PIDControl.cpp -# src/pid_control_nodelet.cpp) add_dependencies(kr_mav_pid_control ${catkin_EXPORTED_TARGETS}) -# target_link_libraries(kr_mav_pid_control ${catkin_LIBRARIES}) - -install( - TARGETS kr_mav_so3_control - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(kr_mav_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +include_directories(include) + +add_library(kr_mav_pid_control SHARED src/pid_control_component.cpp src/PIDControl.cpp) +ament_target_dependencies(kr_mav_pid_control rclcpp rclcpp_components kr_mav_msgs nav_msgs std_msgs tf2 tf2_geometry_msgs) +target_link_libraries(kr_mav_pid_control Eigen3::Eigen) +rclcpp_components_register_nodes(kr_mav_pid_control "PIDControlComponent") + +add_library(kr_mav_so3_control SHARED src/so3_control_component.cpp src/SO3Control.cpp src/so3_trpy_control.cpp) +ament_target_dependencies(kr_mav_so3_control rclcpp rclcpp_components kr_mav_msgs nav_msgs std_msgs geometry_msgs tf2 tf2_geometry_msgs) +target_link_libraries(kr_mav_so3_control Eigen3::Eigen) +rclcpp_components_register_nodes(kr_mav_so3_control "SO3ControlComponent") +rclcpp_components_register_nodes(kr_mav_so3_control "SO3TRPYControlComponent") + +install(TARGETS + kr_mav_pid_control + kr_mav_so3_control + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -# install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(FILES nodelet_plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) -if(CATKIN_ENABLE_TESTING) - find_package(catkin REQUIRED COMPONENTS - roscpp + include_directories(include) + ament_add_gtest_executable(so3_control_component_test test/so3_control_component_test.cpp) + ament_target_dependencies(so3_control_component_test std_msgs - nav_msgs kr_mav_msgs - rostest + nav_msgs + rclcpp ) - include_directories(include ${catkin_INCLUDE_DIRS}) - include_directories(${GTEST_INCLUDE_DIRS}) - - add_executable(so3_control_nodelet_test test/so3_control_nodelet_test.cpp) - # add_rostest_gtest(so3_control_nodelet_test test/so3_control_nodelet.test test/so3_control_nodelet_test.cpp) - target_link_libraries(so3_control_nodelet_test ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) - - add_rostest(test/so3_control_nodelet.test) + add_launch_test(test/launch/so3_control_component_test.test.py TIMEOUT 80 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() + +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/kr_mav_controllers/README.md b/kr_mav_controllers/README.md new file mode 100644 index 00000000..04026d75 --- /dev/null +++ b/kr_mav_controllers/README.md @@ -0,0 +1,14 @@ +kr_mav_controllers +============= + +Package for SO3 Controllers. + +## Build Package +``` +colcon build --packages-select kr_mav_controllers +``` + +## Run Tests +``` +colcon test --packages-select kr_mav_controllers && colcon test-result --all --verbose +``` diff --git a/kr_mav_controllers/cfg/SO3.cfg b/kr_mav_controllers/cfg/SO3.cfg deleted file mode 100755 index 914be01d..00000000 --- a/kr_mav_controllers/cfg/SO3.cfg +++ /dev/null @@ -1,51 +0,0 @@ -#!/usr/bin/env python3 -PACKAGE = "kr_mav_controllers" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -tg = gen.add_group("Translation Gains") -# World Position Gains -tg.add("kp_x", double_t, 1, "World x position gain", 7.4, 0.0, 20.0) -tg.add("kp_y", double_t, 1, "World y position gain", 7.4, 0.0, 20.0) -tg.add("kp_z", double_t, 1, "World z position gain", 10.4, 0.0, 20.0) -# World Derivative Gai -tg.add("kd_x", double_t, 1, "World x derivative gain", 4.8, 0.0, 20.0) -tg.add("kd_y", double_t, 1, "World y derivative gain", 4.8, 0.0, 20.0) -tg.add("kd_z", double_t, 1, "World z derivative gain", 6.0, 0.0, 20.0) - -ig = gen.add_group("Integral Gains") -# World Integral Gains -ig.add("ki_x", double_t, 2, "World x integral gain", 0.0, 0.0, 0.2) -ig.add("ki_y", double_t, 2, "World y integral gain", 0.0, 0.0, 0.2) -ig.add("ki_z", double_t, 2, "World z integral gain", 0.0, 0.0, 0.2) -# Body Integral Gains -ig.add("kib_x", double_t, 2, "Body x integral gain", 0.0, 0.0, 0.2) -ig.add("kib_y", double_t, 2, "Body y integral gain", 0.0, 0.0, 0.2) -ig.add("kib_z", double_t, 2, "Body z integral gain", 0.0, 0.0, 0.2) - -ag = gen.add_group("Attitude Gains") -# Rotation Gains -ag.add("rot_x", double_t, 4, "Rotation x gain", 1.5, 0.0, 3.00) -ag.add("rot_y", double_t, 4, "Rotation y gain", 1.5, 0.0, 3.00) -ag.add("rot_z", double_t, 4, "Rotation z gain", 1.0, 0.0, 3.00) -# Angular Gains -ag.add("ang_x", double_t, 4, "Angular x gain", 0.13, 0.0, 1.00) -ag.add("ang_y", double_t, 4, "Angular y gain", 0.13, 0.0, 1.00) -ag.add("ang_z", double_t, 4, "Angular z gain", 0.10, 0.0, 1.00) - -# Low level corrections -corr = gen.add_group("Corrections") -# TODO: Determine limits -corr.add("kf_correction", double_t, 8, "kf", 0.0, 0.0, 0.0) -corr.add("roll_correction", double_t, 8, "roll", 0.0, 0.0, 0.0) -corr.add("pitch_correction", double_t, 8, "pitch", 0.0, 0.0, 0.0) - -# Misc -misc = gen.add_group("Maxes and limits") -misc.add("max_pos_int", double_t, 16, "World max integral", 0.0, 0.0, 4.00) -misc.add("max_pos_int_b", double_t, 16, "Body max integral", 0.0, 0.0, 4.00) -misc.add("max_tilt_angle", double_t, 16, "Max tilt angle", 3.14, 0.0, 3.14) - -exit(gen.generate(PACKAGE, "kr_mav_controllers", "SO3")) diff --git a/kr_mav_controllers/include/kr_mav_controllers/PIDControl.hpp b/kr_mav_controllers/include/kr_mav_controllers/PIDControl.hpp index a6be01f2..9d5f3e77 100644 --- a/kr_mav_controllers/include/kr_mav_controllers/PIDControl.hpp +++ b/kr_mav_controllers/include/kr_mav_controllers/PIDControl.hpp @@ -1,5 +1,5 @@ -#ifndef PID_CONTROL_H -#define PID_CONTROL_H +#ifndef PID_CONTROL_HPP +#define PID_CONTROL_HPP #include @@ -22,7 +22,7 @@ class PIDControl const Eigen::Vector4f &getControls(void); - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: // Inputs for the controller @@ -39,4 +39,4 @@ class PIDControl Eigen::Vector4f trpy_; }; -#endif +#endif // PID_CONTROL_HPP diff --git a/kr_mav_controllers/include/kr_mav_controllers/SO3Control.h b/kr_mav_controllers/include/kr_mav_controllers/SO3Control.hpp similarity index 93% rename from kr_mav_controllers/include/kr_mav_controllers/SO3Control.h rename to kr_mav_controllers/include/kr_mav_controllers/SO3Control.hpp index a30350c1..91b6bc8b 100644 --- a/kr_mav_controllers/include/kr_mav_controllers/SO3Control.h +++ b/kr_mav_controllers/include/kr_mav_controllers/SO3Control.hpp @@ -1,5 +1,5 @@ -#ifndef SO3_CONTROL_H -#define SO3_CONTROL_H +#ifndef SO3_CONTROL_HPP +#define SO3_CONTROL_HPP #include @@ -27,7 +27,7 @@ class SO3Control const Eigen::Quaternionf &getComputedOrientation(); const Eigen::Vector3f &getComputedAngularVelocity(); - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: // Inputs for the controller @@ -48,4 +48,4 @@ class SO3Control Eigen::Vector3f pos_int_b_; }; -#endif +#endif // SO3_CONTROL_HPP diff --git a/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp b/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp index 55d57143..623d5071 100644 --- a/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp +++ b/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp @@ -1,16 +1,17 @@ -#include -#include -#include -#include -#include -#include -#include +#include "kr_mav_msgs/msg/corrections.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_mav_msgs/msg/so3_command.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" -class SO3ControlTester +using namespace std::chrono_literals; + +class SO3ControlTester : public rclcpp::Node { public: SO3ControlTester(); - void so3CommandCallback(const kr_mav_msgs::SO3Command::ConstPtr &msg); + void so3CommandCallback(const kr_mav_msgs::msg::SO3Command::SharedPtr msg); void publish_single_position_command(); void publish_enable_motors(bool flag); void populate_position_cmd_vector(int x_gain, int y_gain, int z_gain, uint8_t use_msg_gains_flags, float yaw_dot); @@ -20,27 +21,30 @@ class SO3ControlTester void reset_so3_cmd_pointer(); void publish_corrections(float kf_correction, float (&angle_corrections)[2]); bool is_so3_cmd_publisher_active(); - kr_mav_msgs::SO3Command::Ptr so3_cmd_; + kr_mav_msgs::msg::SO3Command::SharedPtr so3_cmd_; bool so3_command_received_ = false; std::mutex mutex; private: - ros::NodeHandle nh_; - ros::Publisher position_cmd_pub_, enable_motors_pub_, odom_pub_, corrections_pub_; - ros::Subscriber so3_cmd_sub_; - std::vector position_cmds; - std::vector odom_msgs; + rclcpp::Publisher::SharedPtr position_cmd_pub_; + rclcpp::Publisher::SharedPtr enable_motors_pub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr corrections_pub_; + rclcpp::Subscription::SharedPtr so3_cmd_sub_; + std::vector position_cmds; + std::vector odom_msgs; std::array angles = {0.0, 0.314, 0.628, 0.942, 1.257, 1.571, 1.885, 2.199, 2.513, 2.827, 3.142}; }; -SO3ControlTester::SO3ControlTester() : nh_("") +SO3ControlTester::SO3ControlTester() : Node("so3_control_tester") { - position_cmd_pub_ = nh_.advertise("position_cmd", 5, true); - so3_cmd_sub_ = nh_.subscribe("so3_cmd", 5, &SO3ControlTester::so3CommandCallback, this); - enable_motors_pub_ = nh_.advertise("motors", 5, true); - odom_pub_ = nh_.advertise("odom", 5, true); - corrections_pub_ = nh_.advertise("corrections", 5, true); + position_cmd_pub_ = this->create_publisher("position_cmd", 5); + enable_motors_pub_ = this->create_publisher("motors", 5); + odom_pub_ = this->create_publisher("odom", 5); + corrections_pub_ = this->create_publisher("corrections", 5); + so3_cmd_sub_ = this->create_subscription( + "so3_cmd", 5, std::bind(&SO3ControlTester::so3CommandCallback, this, std::placeholders::_1)); } /* @@ -50,12 +54,12 @@ SO3ControlTester::SO3ControlTester() : nh_("") bool SO3ControlTester::is_so3_cmd_publisher_active() { bool flag = false; - if(so3_cmd_sub_.getNumPublishers() > 0) + if(so3_cmd_sub_->get_publisher_count() > 0) flag = true; else { - ros::Duration(1.0).sleep(); - if(so3_cmd_sub_.getNumPublishers() > 0) + rclcpp::sleep_for(1s); + if(so3_cmd_sub_->get_publisher_count() > 0) flag = true; else flag = false; @@ -66,20 +70,20 @@ bool SO3ControlTester::is_so3_cmd_publisher_active() /* * @brief Callback function to hand so3 command messages from nodelet. * - * @param msg Type: kr_mav_msgs::SO3Command::ConstPtr + * @param[in] msg Type: kr_mav_msgs::SO3Command::ConstPtr */ -void SO3ControlTester::so3CommandCallback(const kr_mav_msgs::SO3Command::ConstPtr &msg) +void SO3ControlTester::so3CommandCallback(const kr_mav_msgs::msg::SO3Command::SharedPtr msg) { std::lock_guard lock(mutex); so3_command_received_ = true; if(!so3_cmd_) { - so3_cmd_ = boost::make_shared(*msg); + so3_cmd_ = std::make_shared(*msg); } else { so3_cmd_.reset(); - so3_cmd_ = boost::make_shared(*msg); + so3_cmd_ = std::make_shared(*msg); } } @@ -99,7 +103,7 @@ void SO3ControlTester::reset_so3_cmd_pointer() */ void SO3ControlTester::publish_single_position_command() { - kr_mav_msgs::PositionCommand cmd; + auto cmd = kr_mav_msgs::msg::PositionCommand(); cmd.position.x = 1.0; cmd.position.y = 0.2; cmd.position.z = 5.0; @@ -121,30 +125,30 @@ void SO3ControlTester::publish_single_position_command() cmd.kv[1] = 0.1; cmd.kv[2] = 0.1; cmd.use_msg_gains_flags = 0; - position_cmd_pub_.publish(cmd); + position_cmd_pub_->publish(cmd); } /* * @brief Function to publish message over the topic `motors` * - * @param flag boolean flag message to enable/disable motors + * @param[in] flag boolean flag message to enable/disable motors */ void SO3ControlTester::publish_enable_motors(bool flag) { - std_msgs::Bool msg; + auto msg = std_msgs::msg::Bool(); msg.data = flag; - enable_motors_pub_.publish(msg); + enable_motors_pub_->publish(msg); } /* * @brief Function that populates the a vector of `kr_mav_messages::PositionCommand` * Generates position commands based on sine and cosine functions * - * @param x_gain int, max x-position - * @param y_gain int, max y-position - * @param z_gain int, max z_position - * @param use_msg_gains_flags uint8_t, indicates which gains to use from the message - * @param yaw_dot float, desired yaw_dot + * @param[in] x_gain int, max x-position + * @param[in] y_gain int, max y-position + * @param[in] z_gain int, max z_position + * @param[in] use_msg_gains_flags uint8_t, indicates which gains to use from the message + * @param[in] yaw_dot float, desired yaw_dot */ void SO3ControlTester::populate_position_cmd_vector(int x_gain, int y_gain, int z_gain, uint8_t use_msg_gains_flags, float yaw_dot) @@ -160,7 +164,7 @@ void SO3ControlTester::populate_position_cmd_vector(int x_gain, int y_gain, int float z_sin = z_gain * std::sin(angles[i]); float z_cos = z_gain * std::cos(angles[i]); - kr_mav_msgs::PositionCommand cmd; + auto cmd = kr_mav_msgs::msg::PositionCommand(); cmd.position.x = x_sin; cmd.position.y = y_sin; cmd.position.z = z_sin; @@ -189,14 +193,14 @@ void SO3ControlTester::populate_position_cmd_vector(int x_gain, int y_gain, int /* * @brief Function to publish position command indexed from the vector of `kr_mav_msgs::PositionCommand` msgs * - * @param index int + * @param[in] index int */ void SO3ControlTester::publish_position_command(int index) { int position_cmds_vector_size = position_cmds.size(); if(index < position_cmds_vector_size) { - position_cmd_pub_.publish(position_cmds[index]); + position_cmd_pub_->publish(position_cmds[index]); } } @@ -207,7 +211,7 @@ void SO3ControlTester::populate_odom_msgs() { odom_msgs.clear(); - nav_msgs::Odometry msg; + auto msg = nav_msgs::msg::Odometry(); msg.header.frame_id = "quadrotor"; msg.pose.pose.position.x = 0.0; msg.pose.pose.position.y = 0.0; @@ -260,30 +264,30 @@ void SO3ControlTester::populate_odom_msgs() /* * @brief Function to publish odom message index from a vector of `nav_msgs::Odometry` msgs * - * @param index int + * @param[in] index int */ void SO3ControlTester::publish_odom_msg(int index) { int odom_vector_size = odom_msgs.size(); if(index < odom_vector_size) { - odom_pub_.publish(odom_msgs[index]); + odom_pub_->publish(odom_msgs[index]); } } /* * @brief Function to publish corrections message of type `kr_mav_msgs::Corrections` * - * @param kf_correction float - * @param angle_corrections float[2] + * @param[in] kf_correction float + * @param[in] angle_corrections float[2] */ void SO3ControlTester::publish_corrections(float kf_correction, float (&angle_corrections)[2]) { - kr_mav_msgs::Corrections msg; + auto msg = kr_mav_msgs::msg::Corrections(); msg.kf_correction = kf_correction; msg.angle_corrections[0] = angle_corrections[0]; msg.angle_corrections[1] = angle_corrections[1]; - corrections_pub_.publish(msg); + corrections_pub_->publish(msg); } /* diff --git a/kr_mav_controllers/nodelet_plugin.xml b/kr_mav_controllers/nodelet_plugin.xml deleted file mode 100644 index f95a1e82..00000000 --- a/kr_mav_controllers/nodelet_plugin.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - kr_mav_so3_controllers - - - - - - kr_mav_so3_controllers with thrust, roll, pitch, yaw output - - - - - diff --git a/kr_mav_controllers/package.xml b/kr_mav_controllers/package.xml index 4008c415..69541ff2 100644 --- a/kr_mav_controllers/package.xml +++ b/kr_mav_controllers/package.xml @@ -1,30 +1,31 @@ - + + + kr_mav_controllers 1.0.0 kr_mav_controllers Kartik Mohta Michael Watterson - + Pranav Shah BSD Kartik Mohta - catkin + ament_cmake - dynamic_reconfigure - geometry_msgs + rclcpp + rclcpp_components kr_mav_msgs nav_msgs - nodelet - roscpp std_msgs - tf + geometry_msgs + tf2 + tf2_geometry_msgs - rostest - gtest + ament_cmake_gtest + launch_testing_ament_cmake - + ament_cmake - diff --git a/kr_mav_controllers/src/PIDControl.cpp b/kr_mav_controllers/src/PIDControl.cpp index 4bfb2a5f..526c1352 100644 --- a/kr_mav_controllers/src/PIDControl.cpp +++ b/kr_mav_controllers/src/PIDControl.cpp @@ -1,5 +1,6 @@ +#include "kr_mav_controllers/PIDControl.hpp" + #include -#include PIDControl::PIDControl() : mass_(0.5), g_(9.81), yaw_int_(0.0), max_pos_int_(0.5) {} diff --git a/kr_mav_controllers/src/SO3Control.cpp b/kr_mav_controllers/src/SO3Control.cpp index d9da233b..861b45e1 100644 --- a/kr_mav_controllers/src/SO3Control.cpp +++ b/kr_mav_controllers/src/SO3Control.cpp @@ -1,7 +1,4 @@ -#include "kr_mav_controllers/SO3Control.h" - -#include -#include +#include "kr_mav_controllers/SO3Control.hpp" SO3Control::SO3Control() : mass_(0.5), @@ -73,8 +70,6 @@ void SO3Control::calculateControl(const Eigen::Vector3f &des_pos, const Eigen::V else if(pos_int_(i) < -max_pos_int_) pos_int_(i) = -max_pos_int_; } - // ROS_DEBUG_THROTTLE(2, "Integrated world disturbance compensation [N]: {x: %2.2f, y: %2.2f, z: %2.2f}", pos_int_(0), - // pos_int_(1), pos_int_(2)); Eigen::Quaternionf q(current_orientation_); const Eigen::Vector3f e_pos_b = q.inverse() * e_pos; @@ -89,8 +84,6 @@ void SO3Control::calculateControl(const Eigen::Vector3f &des_pos, const Eigen::V else if(pos_int_b_(i) < -max_pos_int_b_) pos_int_b_(i) = -max_pos_int_b_; } - // ROS_DEBUG_THROTTLE(2, "Integrated body disturbance compensation [N]: {x: %2.2f, y: %2.2f, z: %2.2f}", - // pos_int_b_(0), pos_int_b_(1), pos_int_b_(2)); const Eigen::Vector3f acc_grav = g_ * Eigen::Vector3f::UnitZ(); const Eigen::Vector3f acc_control = kx.asDiagonal() * e_pos + kv.asDiagonal() * e_vel + pos_int_ + des_acc; diff --git a/kr_mav_controllers/src/pid_control_component.cpp b/kr_mav_controllers/src/pid_control_component.cpp new file mode 100644 index 00000000..35381fa3 --- /dev/null +++ b/kr_mav_controllers/src/pid_control_component.cpp @@ -0,0 +1,184 @@ +#include + +#include "kr_mav_controllers/PIDControl.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_mav_msgs/msg/trpy_command.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x) + +class PIDControlComponent : public rclcpp::Node +{ + public: + PIDControlComponent(const rclcpp::NodeOptions &options); + + private: + void publishTRPYCommand(void); + void position_cmd_callback(const kr_mav_msgs::msg::PositionCommand::UniquePtr cmd); + void odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom); + void enable_motors_callback(const std_msgs::msg::Bool::UniquePtr msg); + + PIDControl controller_; + rclcpp::Publisher::SharedPtr trpy_command_pub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr position_cmd_sub_; + rclcpp::Subscription::SharedPtr enable_motors_sub_; + + bool position_cmd_updated_, position_cmd_init_; + std::string frame_id_; + + Eigen::Vector3f des_pos_, des_vel_, des_acc_, kx_, kv_, ki_; + float des_yaw_; + float current_yaw_; + float ki_yaw_; + float max_roll_pitch_; + bool enable_motors_; + bool use_external_yaw_; +}; + +void PIDControlComponent::publishTRPYCommand(void) +{ + float ki_yaw = 0; + Eigen::Vector3f ki = Eigen::Vector3f::Zero(); + // Only enable integral terms when motors are on + if(enable_motors_) + { + ki_yaw = ki_yaw_; + ki = ki_; + } + controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_, kx_, kv_, ki, ki_yaw); + + const Eigen::Vector4f &trpy = controller_.getControls(); + + auto trpy_command = std::make_unique(); + trpy_command->header.stamp = this->now(); + trpy_command->header.frame_id = frame_id_; + if(enable_motors_) + { + trpy_command->thrust = trpy(0); + trpy_command->roll = CLAMP(trpy(1), -max_roll_pitch_, max_roll_pitch_); + trpy_command->pitch = CLAMP(trpy(2), -max_roll_pitch_, max_roll_pitch_); + trpy_command->yaw = trpy(3); + } + trpy_command->aux.current_yaw = current_yaw_; + trpy_command->aux.enable_motors = enable_motors_; + trpy_command->aux.use_external_yaw = use_external_yaw_; + trpy_command_pub_->publish(std::move(trpy_command)); +} + +void PIDControlComponent::position_cmd_callback(const kr_mav_msgs::msg::PositionCommand::UniquePtr cmd) +{ + des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z); + des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); + des_acc_ = Eigen::Vector3f(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z); + kx_ = Eigen::Vector3f(cmd->kx[0], cmd->kx[1], cmd->kx[2]); + kv_ = Eigen::Vector3f(cmd->kv[0], cmd->kv[1], cmd->kv[2]); + + des_yaw_ = cmd->yaw; + position_cmd_updated_ = true; + // position_cmd_init_ = true; + + publishTRPYCommand(); +} + +void PIDControlComponent::odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom) +{ + const Eigen::Vector3f position(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z); + const Eigen::Vector3f velocity(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z); + + current_yaw_ = tf2::getYaw(odom->pose.pose.orientation); + + controller_.setPosition(position); + controller_.setVelocity(velocity); + controller_.setYaw(current_yaw_); + + if(position_cmd_init_) + { + // We set position_cmd_updated_ = false and expect that the + // position_cmd_callback would set it to true since typically a position_cmd + // message would follow an odom message. If not, the position_cmd_callback + // hasn't been called and we publish the so3 command ourselves + // TODO: Fallback to hover if position_cmd hasn't been received for some time + if(!position_cmd_updated_) + publishTRPYCommand(); + position_cmd_updated_ = false; + } +} + +void PIDControlComponent::enable_motors_callback(const std_msgs::msg::Bool::UniquePtr msg) +{ + if(msg->data) + RCLCPP_INFO(this->get_logger(), "Enabling Motors"); + else + RCLCPP_INFO(this->get_logger(), "Disabling Motors"); + + enable_motors_ = msg->data; + + // Reset integrals when toggling motor state + controller_.resetIntegrals(); +} + +PIDControlComponent::PIDControlComponent(const rclcpp::NodeOptions &options) + : Node("pid_control", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + position_cmd_updated_(false), + position_cmd_init_(false), + des_yaw_(0), + current_yaw_(0), + enable_motors_(false), + use_external_yaw_(false) +{ + // RCLCPP_INFO_STREAM(this->get_logger(), "Intra-Process is "<< ( this->get_node_options().use_intra_process_comms() ? + // "ON" : "OFF" )); + this->declare_parameter("quadrotor_name", std::string("quadrotor")); + this->declare_parameter("mass", 0.235); + this->declare_parameter("use_external_yaw", true); + this->declare_parameter("max_roll_pitch", 30.0); + this->declare_parameter("gains/ki/x", 0.0); + this->declare_parameter("gains/ki/y", 0.0); + this->declare_parameter("gains/ki/z", 0.0); + this->declare_parameter("gains/ki/yaw", 0.01); + + std::string quadrotor_name; + this->get_parameter("quadrotor_name", quadrotor_name); + frame_id_ = "/" + quadrotor_name; + + double mass; + this->get_parameter("mass", mass); + controller_.setMass(mass); + controller_.setMaxIntegral(mass * 3); + + this->get_parameter("use_external_yaw", use_external_yaw_); + + double max_roll_pitch; + this->get_parameter("max_roll_pitch", max_roll_pitch); + max_roll_pitch_ = max_roll_pitch; + + double ki_x, ki_y, ki_z, ki_yaw; + this->get_parameter("gains/ki/x", ki_x); + this->get_parameter("gains/ki/y", ki_y); + this->get_parameter("gains/ki/z", ki_z); + this->get_parameter("gains/ki/yaw", ki_yaw); + ki_[0] = ki_x, ki_[1] = ki_y, ki_[2] = ki_z; + ki_yaw_ = ki_yaw; + + trpy_command_pub_ = this->create_publisher("~/trpy_cmd", 10); + + // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos1 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 2), qos_profile); + + odom_sub_ = this->create_subscription( + "~/odom", qos1, std::bind(&PIDControlComponent::odom_callback, this, std::placeholders::_1)); + position_cmd_sub_ = this->create_subscription( + "~/position_cmd", qos1, std::bind(&PIDControlComponent::position_cmd_callback, this, std::placeholders::_1)); + enable_motors_sub_ = this->create_subscription( + "~/motors", qos2, std::bind(&PIDControlComponent::enable_motors_callback, this, std::placeholders::_1)); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(PIDControlComponent) diff --git a/kr_mav_controllers/src/pid_control_nodelet.cpp b/kr_mav_controllers/src/pid_control_nodelet.cpp deleted file mode 100644 index a6de961b..00000000 --- a/kr_mav_controllers/src/pid_control_nodelet.cpp +++ /dev/null @@ -1,173 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x) - -class PIDControlNodelet : public nodelet::Nodelet -{ - public: - PIDControlNodelet() - : position_cmd_updated_(false), - position_cmd_init_(false), - des_yaw_(0), - current_yaw_(0), - enable_motors_(false), - use_external_yaw_(false) - { - } - - void onInit(void); - - private: - void publishTRPYCommand(void); - void position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - void enable_motors_callback(const std_msgs::Bool::ConstPtr &msg); - - PIDControl controller_; - ros::Publisher trpy_command_pub_; - ros::Subscriber odom_sub_; - ros::Subscriber position_cmd_sub_; - ros::Subscriber enable_motors_sub_; - - bool position_cmd_updated_, position_cmd_init_; - std::string frame_id_; - - Eigen::Vector3f des_pos_, des_vel_, des_acc_, kx_, kv_, ki_; - float des_yaw_; - float current_yaw_; - float ki_yaw_; - float max_roll_pitch_; - bool enable_motors_; - bool use_external_yaw_; -}; - -void PIDControlNodelet::publishTRPYCommand(void) -{ - float ki_yaw = 0; - Eigen::Vector3f ki = Eigen::Vector3f::Zero(); - // Only enable integral terms when motors are on - if(enable_motors_) - { - ki_yaw = ki_yaw_; - ki = ki_; - } - controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_, kx_, kv_, ki, ki_yaw); - - const Eigen::Vector4f &trpy = controller_.getControls(); - - kr_mav_msgs::TRPYCommand::Ptr trpy_command(new kr_mav_msgs::TRPYCommand); - trpy_command->header.stamp = ros::Time::now(); - trpy_command->header.frame_id = frame_id_; - if(enable_motors_) - { - trpy_command->thrust = trpy(0); - trpy_command->roll = CLAMP(trpy(1), -max_roll_pitch_, max_roll_pitch_); - trpy_command->pitch = CLAMP(trpy(2), -max_roll_pitch_, max_roll_pitch_); - trpy_command->yaw = trpy(3); - } - trpy_command->aux.current_yaw = current_yaw_; - trpy_command->aux.enable_motors = enable_motors_; - trpy_command->aux.use_external_yaw = use_external_yaw_; - trpy_command_pub_.publish(trpy_command); -} - -void PIDControlNodelet::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) -{ - des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z); - des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); - des_acc_ = Eigen::Vector3f(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z); - kx_ = Eigen::Vector3f(cmd->kx[0], cmd->kx[1], cmd->kx[2]); - kv_ = Eigen::Vector3f(cmd->kv[0], cmd->kv[1], cmd->kv[2]); - - des_yaw_ = cmd->yaw; - position_cmd_updated_ = true; - // position_cmd_init_ = true; - - publishTRPYCommand(); -} - -void PIDControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) -{ - const Eigen::Vector3f position(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z); - const Eigen::Vector3f velocity(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z); - - current_yaw_ = tf::getYaw(odom->pose.pose.orientation); - - controller_.setPosition(position); - controller_.setVelocity(velocity); - controller_.setYaw(current_yaw_); - - if(position_cmd_init_) - { - // We set position_cmd_updated_ = false and expect that the - // position_cmd_callback would set it to true since typically a position_cmd - // message would follow an odom message. If not, the position_cmd_callback - // hasn't been called and we publish the so3 command ourselves - // TODO: Fallback to hover if position_cmd hasn't been received for some time - if(!position_cmd_updated_) - publishTRPYCommand(); - position_cmd_updated_ = false; - } -} - -void PIDControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr &msg) -{ - if(msg->data) - ROS_INFO("Enabling motors"); - else - ROS_INFO("Disabling motors"); - - enable_motors_ = msg->data; - - // Reset integrals when toggling motor state - controller_.resetIntegrals(); -} - -void PIDControlNodelet::onInit(void) -{ - ros::NodeHandle n(getPrivateNodeHandle()); - - std::string quadrotor_name; - n.param("quadrotor_name", quadrotor_name, std::string("quadrotor")); - frame_id_ = "/" + quadrotor_name; - - double mass; - n.param("mass", mass, 0.235); - controller_.setMass(mass); - controller_.setMaxIntegral(mass * 3); - - n.param("use_external_yaw", use_external_yaw_, true); - - double max_roll_pitch; - n.param("max_roll_pitch", max_roll_pitch, 30.0); - max_roll_pitch_ = max_roll_pitch; - - double ki_x, ki_y, ki_z, ki_yaw; - n.param("gains/ki/x", ki_x, 0.0); - n.param("gains/ki/y", ki_y, 0.0); - n.param("gains/ki/z", ki_z, 0.0); - n.param("gains/ki/yaw", ki_yaw, 0.01); - ki_[0] = ki_x, ki_[1] = ki_y, ki_[2] = ki_z; - ki_yaw_ = ki_yaw; - - trpy_command_pub_ = n.advertise("trpy_cmd", 10); - - odom_sub_ = n.subscribe("odom", 10, &PIDControlNodelet::odom_callback, this, ros::TransportHints().tcpNoDelay()); - position_cmd_sub_ = n.subscribe("position_cmd", 10, &PIDControlNodelet::position_cmd_callback, this, - ros::TransportHints().tcpNoDelay()); - - enable_motors_sub_ = - n.subscribe("motors", 2, &PIDControlNodelet::enable_motors_callback, this, ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(PIDControlNodelet, nodelet::Nodelet); diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp new file mode 100644 index 00000000..823a8946 --- /dev/null +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -0,0 +1,649 @@ +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "kr_mav_controllers/SO3Control.hpp" +#include "kr_mav_msgs/msg/corrections.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_mav_msgs/msg/so3_command.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +class SO3ControlComponent : public rclcpp::Node +{ + public: + SO3ControlComponent(const rclcpp::NodeOptions &options); + + private: + void publishSO3Command(); + void position_cmd_callback(const kr_mav_msgs::msg::PositionCommand::UniquePtr cmd); + void odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom); + void enable_motors_callback(const std_msgs::msg::Bool::UniquePtr msg); + void corrections_callback(const kr_mav_msgs::msg::Corrections::UniquePtr msg); + rcl_interfaces::msg::SetParametersResult cfg_callback(std::vector parameters); + + SO3Control controller_; + rclcpp::Publisher::SharedPtr so3_command_pub_; + rclcpp::Publisher::SharedPtr command_viz_pub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr position_cmd_sub_; + rclcpp::Subscription::SharedPtr enable_motors_sub_; + rclcpp::Subscription::SharedPtr corrections_sub_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfigure_handle_; + + bool position_cmd_updated_, position_cmd_init_; + std::string frame_id_; + + Eigen::Vector3f des_pos_, des_vel_, des_acc_, des_jrk_, kx_, kv_, config_kx_, config_kv_, config_ki_, config_kib_; + float des_yaw_, des_yaw_dot_; + float current_yaw_; + bool enable_motors_, use_external_yaw_, have_odom_; + float kr_[3], kom_[3], corrections_[3]; + float mass_; + const float g_; + Eigen::Quaternionf current_orientation_; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Need this since we have SO3Control which needs aligned pointer +}; + +void SO3ControlComponent::publishSO3Command() +{ + if(!have_odom_) + { + RCLCPP_WARN(this->get_logger(), "No Odometry! Not publishing SO3Command."); + return; + } + + Eigen::Vector3f ki = Eigen::Vector3f::Zero(); + Eigen::Vector3f kib = Eigen::Vector3f::Zero(); + if(enable_motors_) + { + ki = config_ki_; + kib = config_kib_; + } + + controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_jrk_, des_yaw_, des_yaw_dot_, kx_, kv_, ki, kib); + + const Eigen::Vector3f &force = controller_.getComputedForce(); + const Eigen::Quaternionf &orientation = controller_.getComputedOrientation(); + const Eigen::Vector3f &ang_vel = controller_.getComputedAngularVelocity(); + + auto so3_command = std::make_unique(); + so3_command->header.stamp = this->now(); + so3_command->header.frame_id = frame_id_; + so3_command->force.x = force(0); + so3_command->force.y = force(1); + so3_command->force.z = force(2); + so3_command->orientation.x = orientation.x(); + so3_command->orientation.y = orientation.y(); + so3_command->orientation.z = orientation.z(); + so3_command->orientation.w = orientation.w(); + so3_command->angular_velocity.x = ang_vel(0); + so3_command->angular_velocity.y = ang_vel(1); + so3_command->angular_velocity.z = ang_vel(2); + for(int i = 0; i < 3; i++) + { + so3_command->kr[i] = kr_[i]; + so3_command->kom[i] = kom_[i]; + } + so3_command->aux.current_yaw = current_yaw_; + so3_command->aux.kf_correction = corrections_[0]; + so3_command->aux.angle_corrections[0] = corrections_[1]; + so3_command->aux.angle_corrections[1] = corrections_[2]; + so3_command->aux.enable_motors = enable_motors_; + so3_command->aux.use_external_yaw = use_external_yaw_; + + auto cmd_viz_msg = std::make_unique(); + cmd_viz_msg->header = so3_command->header; + cmd_viz_msg->pose.position.x = des_pos_(0); + cmd_viz_msg->pose.position.y = des_pos_(1); + cmd_viz_msg->pose.position.z = des_pos_(2); + cmd_viz_msg->pose.orientation.x = orientation.x(); + cmd_viz_msg->pose.orientation.y = orientation.y(); + cmd_viz_msg->pose.orientation.z = orientation.z(); + cmd_viz_msg->pose.orientation.w = orientation.w(); + command_viz_pub_->publish(std::move(cmd_viz_msg)); + + so3_command_pub_->publish(std::move(so3_command)); +} + +void SO3ControlComponent::position_cmd_callback(const kr_mav_msgs::msg::PositionCommand::UniquePtr cmd) +{ + des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z); + des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); + des_acc_ = Eigen::Vector3f(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z); + des_jrk_ = Eigen::Vector3f(cmd->jerk.x, cmd->jerk.y, cmd->jerk.z); + + // Check use_msg_gains_flag to decide whether to use gains from the msg or config + kx_[0] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_X) ? cmd->kx[0] : config_kx_[0]; + kx_[1] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_Y) ? cmd->kx[1] : config_kx_[1]; + kx_[2] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_Z) ? cmd->kx[2] : config_kx_[2]; + kv_[0] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_X) ? cmd->kv[0] : config_kv_[0]; + kv_[1] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_Y) ? cmd->kv[1] : config_kv_[1]; + kv_[2] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_Z) ? cmd->kv[2] : config_kv_[2]; + + des_yaw_ = cmd->yaw; + des_yaw_dot_ = cmd->yaw_dot; + position_cmd_updated_ = true; + // position_cmd_init_ = true; + + publishSO3Command(); +} + +void SO3ControlComponent::odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom) +{ + have_odom_ = true; + + frame_id_ = odom->header.frame_id; + + const Eigen::Vector3f position(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z); + const Eigen::Vector3f velocity(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z); + + current_yaw_ = tf2::getYaw(odom->pose.pose.orientation); + + current_orientation_ = Eigen::Quaternionf(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, + odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); + + controller_.setPosition(position); + controller_.setVelocity(velocity); + controller_.setCurrentOrientation(current_orientation_); + + if(position_cmd_init_) + { + // We set position_cmd_updated_ = false and expect that the + // position_cmd_callback would set it to true since typically a position_cmd + // message would follow an odom message. If not, the position_cmd_callback + // hasn't been called and we publish the so3 command ourselves + // TODO: Fallback to hover if position_cmd hasn't been received for some time + if(!position_cmd_updated_) + publishSO3Command(); + position_cmd_updated_ = false; + } +} + +void SO3ControlComponent::enable_motors_callback(const std_msgs::msg::Bool::UniquePtr msg) +{ + if(msg->data) + RCLCPP_INFO(this->get_logger(), "Enabling motors"); + else + RCLCPP_INFO(this->get_logger(), "Disabling motors"); + + enable_motors_ = msg->data; + // Reset integral when toggling motor state + controller_.resetIntegrals(); +} + +void SO3ControlComponent::corrections_callback(const kr_mav_msgs::msg::Corrections::UniquePtr msg) +{ + corrections_[0] = msg->kf_correction; + corrections_[1] = msg->angle_corrections[0]; + corrections_[2] = msg->angle_corrections[1]; +} + +rcl_interfaces::msg::SetParametersResult SO3ControlComponent::cfg_callback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for(auto ¶meter : parameters) + { + const auto &name = parameter.get_name(); + + if(name == "kp_x") + { + config_kx_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kp_y") + { + config_kx_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kp_z") + { + config_kx_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kd_x") + { + config_kv_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kd_y") + { + config_kv_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kd_z") + { + config_kv_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "ki_x") + { + config_ki_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_y") + { + config_ki_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_z") + { + config_ki_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "kib_x") + { + config_kib_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_y") + { + config_kib_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_z") + { + config_kib_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "rot_x") + { + kr_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "rot_y") + { + kr_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "rot_z") + { + kr_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "ang_x") + { + kom_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "ang_y") + { + kom_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "ang_z") + { + kom_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "kf_correction") + { + corrections_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + } + else if(name == "roll_correction") + { + corrections_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + } + else if(name == "pitch_correction") + { + corrections_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + } + else if(name == "max_pos_int") + { + controller_.setMaxIntegral(parameter.as_double()); + RCLCPP_INFO(this->get_logger(), "World max integral set to: %2.2g", parameter.as_double()); + } + else if(name == "max_pos_int_b") + { + controller_.setMaxIntegralBody(parameter.as_double()); + RCLCPP_INFO(this->get_logger(), "Body max integral set to: %2.2g", parameter.as_double()); + } + else if(name == "max_tilt_angle") + { + controller_.setMaxTiltAngle(parameter.as_double()); + RCLCPP_INFO(this->get_logger(), "Max tilt angle set to: %2.2g", parameter.as_double()); + } + else + { + RCLCPP_WARN_STREAM(this->get_logger(), "kr_mav_controllers dynamic reconfigure called with invalid parameter"); + result.successful = false; + return result; + } + } + result.successful = true; + return result; +} + +SO3ControlComponent::SO3ControlComponent(const rclcpp::NodeOptions &options) + : Node("so3_control", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + position_cmd_updated_(false), + position_cmd_init_(false), + des_yaw_(0), + des_yaw_dot_(0), + current_yaw_(0), + enable_motors_(false), + use_external_yaw_(false), + have_odom_(false), + g_(9.81), + current_orientation_(Eigen::Quaternionf::Identity()) +{ + controller_.resetIntegrals(); + + this->declare_parameter("quadrotor_name", std::string("quadrotor")); + this->declare_parameter("mass", 0.5f); + + std::string quadrotor_name; + this->get_parameter("quadrotor_name", quadrotor_name); + frame_id_ = "/" + quadrotor_name; + this->get_parameter("mass", mass_); + + controller_.setMass(mass_); + controller_.setGravity(g_); + + this->declare_parameter("use_external_yaw", true); + this->get_parameter("use_external_yaw", use_external_yaw_); + + // Dynamic parameter reconfiguration definitions + + // Translation Gains: World Position Gains + rcl_interfaces::msg::ParameterDescriptor desc_kp_x; + rcl_interfaces::msg::FloatingPointRange r_kp_x; + r_kp_x.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kp_x.type = 3; // PARAMETER_DOUBLE + desc_kp_x.description = std::string("World x position gain"); + desc_kp_x.floating_point_range = {r_kp_x}; + this->declare_parameter("kp_x", 7.4f, desc_kp_x); + + rcl_interfaces::msg::ParameterDescriptor desc_kp_y; + rcl_interfaces::msg::FloatingPointRange r_kp_y; + r_kp_y.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kp_y.type = 3; // PARAMETER_DOUBLE + desc_kp_y.description = std::string("World y position gain"); + desc_kp_y.floating_point_range = {r_kp_y}; + this->declare_parameter("kp_y", 7.4f, desc_kp_y); + + rcl_interfaces::msg::ParameterDescriptor desc_kp_z; + rcl_interfaces::msg::FloatingPointRange r_kp_z; + r_kp_z.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kp_z.type = 3; // PARAMETER_DOUBLE + desc_kp_z.description = std::string("World z position gain"); + desc_kp_z.floating_point_range = {r_kp_z}; + this->declare_parameter("kp_z", 10.4f, desc_kp_z); + + // Translation Gains: World Derivative Gains + rcl_interfaces::msg::ParameterDescriptor desc_kd_x; + rcl_interfaces::msg::FloatingPointRange r_kd_x; + r_kd_x.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kd_x.type = 3; // PARAMETER_DOUBLE + desc_kd_x.description = std::string("World x derivative gain"); + desc_kd_x.floating_point_range = {r_kd_x}; + this->declare_parameter("kd_x", 4.8f, desc_kd_x); + + rcl_interfaces::msg::ParameterDescriptor desc_kd_y; + rcl_interfaces::msg::FloatingPointRange r_kd_y; + r_kd_y.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kd_y.type = 3; // PARAMETER_DOUBLE + desc_kd_y.description = std::string("World y derivative gain"); + desc_kd_y.floating_point_range = {r_kd_y}; + this->declare_parameter("kd_y", 4.8f, desc_kd_y); + + rcl_interfaces::msg::ParameterDescriptor desc_kd_z; + rcl_interfaces::msg::FloatingPointRange r_kd_z; + r_kd_z.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kd_z.type = 3; // PARAMETER_DOUBLE + desc_kd_z.description = std::string("World z derivative gain"); + desc_kd_z.floating_point_range = {r_kd_z}; + this->declare_parameter("kd_z", 6.0f, desc_kd_z); + + // Integral Gains: World Integral Gains + rcl_interfaces::msg::ParameterDescriptor desc_ki_x; + rcl_interfaces::msg::FloatingPointRange r_ki_x; + r_ki_x.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_ki_x.type = 3; // PARAMETER_DOUBLE + desc_ki_x.description = std::string("World x integral gain"); + desc_ki_x.floating_point_range = {r_ki_x}; + this->declare_parameter("ki_x", 0.0f, desc_ki_x); + + rcl_interfaces::msg::ParameterDescriptor desc_ki_y; + rcl_interfaces::msg::FloatingPointRange r_ki_y; + r_ki_y.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_ki_y.type = 3; // PARAMETER_DOUBLE + desc_ki_y.description = std::string("World y integral gain"); + desc_ki_y.floating_point_range = {r_ki_y}; + this->declare_parameter("ki_y", 0.0f, desc_ki_y); + + rcl_interfaces::msg::ParameterDescriptor desc_ki_z; + rcl_interfaces::msg::FloatingPointRange r_ki_z; + r_ki_z.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_ki_z.type = 3; // PARAMETER_DOUBLE + desc_ki_z.description = std::string("World z integral gain"); + desc_ki_z.floating_point_range = {r_ki_z}; + this->declare_parameter("ki_z", 0.0f, desc_ki_z); + + // Integral Gains: Body Integral Gains + rcl_interfaces::msg::ParameterDescriptor desc_kib_x; + rcl_interfaces::msg::FloatingPointRange r_kib_x; + r_kib_x.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_kib_x.type = 3; // PARAMETER_DOUBLE + desc_kib_x.description = std::string("Body x integral gain"); + desc_kib_x.floating_point_range = {r_kib_x}; + this->declare_parameter("kib_x", 0.0f, desc_kib_x); + + rcl_interfaces::msg::ParameterDescriptor desc_kib_y; + rcl_interfaces::msg::FloatingPointRange r_kib_y; + r_kib_y.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_kib_y.type = 3; // PARAMETER_DOUBLE + desc_kib_y.description = std::string("Body y integral gain"); + desc_kib_y.floating_point_range = {r_kib_y}; + this->declare_parameter("kib_y", 0.0f, desc_kib_y); + + rcl_interfaces::msg::ParameterDescriptor desc_kib_z; + rcl_interfaces::msg::FloatingPointRange r_kib_z; + r_kib_z.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_kib_z.type = 3; // PARAMETER_DOUBLE + desc_kib_z.description = std::string("Body z integral gain"); + desc_kib_z.floating_point_range = {r_kib_z}; + this->declare_parameter("kib_z", 0.0f, desc_kib_z); + + // Attitude Gains: Rotation Gains + rcl_interfaces::msg::ParameterDescriptor desc_rot_x; + rcl_interfaces::msg::FloatingPointRange r_rot_x; + r_rot_x.set__from_value(0.0).set__to_value(3.00).set__step(0); + desc_rot_x.type = 3; // PARAMETER_DOUBLE + desc_rot_x.description = std::string("Rotation x gain"); + desc_rot_x.floating_point_range = {r_rot_x}; + this->declare_parameter("rot_x", 1.5f, desc_rot_x); + + rcl_interfaces::msg::ParameterDescriptor desc_rot_y; + rcl_interfaces::msg::FloatingPointRange r_rot_y; + r_rot_y.set__from_value(0.0).set__to_value(3.00).set__step(0); + desc_rot_y.type = 3; // PARAMETER_DOUBLE + desc_rot_y.description = std::string("Rotation y gain"); + desc_rot_y.floating_point_range = {r_rot_y}; + this->declare_parameter("rot_y", 1.5f, desc_rot_y); + + rcl_interfaces::msg::ParameterDescriptor desc_rot_z; + rcl_interfaces::msg::FloatingPointRange r_rot_z; + r_rot_z.set__from_value(0.0).set__to_value(3.00).set__step(0); + desc_rot_z.type = 3; // PARAMETER_DOUBLE + desc_rot_z.description = std::string("Rotation z gain"); + desc_rot_z.floating_point_range = {r_rot_z}; + this->declare_parameter("rot_z", 1.0f, desc_rot_z); + + // Attitude Gains: Angular Gains + rcl_interfaces::msg::ParameterDescriptor desc_ang_x; + rcl_interfaces::msg::FloatingPointRange r_ang_x; + r_ang_x.set__from_value(0.0).set__to_value(1.00).set__step(0); + desc_ang_x.type = 3; // PARAMETER_DOUBLE + desc_ang_x.description = std::string("Angular x gain"); + desc_ang_x.floating_point_range = {r_ang_x}; + this->declare_parameter("ang_x", 0.13f, desc_ang_x); + + rcl_interfaces::msg::ParameterDescriptor desc_ang_y; + rcl_interfaces::msg::FloatingPointRange r_ang_y; + r_ang_y.set__from_value(0.0).set__to_value(1.00).set__step(0); + desc_ang_y.type = 3; // PARAMETER_DOUBLE + desc_ang_y.description = std::string("Angular y gain"); + desc_ang_y.floating_point_range = {r_ang_y}; + this->declare_parameter("ang_y", 0.13f, desc_ang_y); + + rcl_interfaces::msg::ParameterDescriptor desc_ang_z; + rcl_interfaces::msg::FloatingPointRange r_ang_z; + r_ang_z.set__from_value(0.0).set__to_value(1.00).set__step(0); + desc_ang_z.type = 3; // PARAMETER_DOUBLE + desc_ang_z.description = std::string("Angular z gain"); + desc_ang_z.floating_point_range = {r_ang_z}; + this->declare_parameter("ang_z", 0.10f, desc_ang_z); + + // Low level Corrections + rcl_interfaces::msg::ParameterDescriptor desc_kf_correction; + desc_kf_correction.type = 3; // PARAMETER_DOUBLE + desc_kf_correction.description = std::string("kf"); + this->declare_parameter("kf_correction", 0.0f, desc_kf_correction); + + rcl_interfaces::msg::ParameterDescriptor desc_roll_correction; + desc_roll_correction.type = 3; // PARAMETER_DOUBLE + desc_roll_correction.description = std::string("roll"); + this->declare_parameter("roll_correction", 0.0f, desc_roll_correction); + + rcl_interfaces::msg::ParameterDescriptor desc_pitch_correction; + desc_pitch_correction.type = 3; // PARAMETER_DOUBLE + desc_pitch_correction.description = std::string("pitch"); + this->declare_parameter("pitch_correction", 0.0f, desc_pitch_correction); + + // Misc Parameters + rcl_interfaces::msg::ParameterDescriptor desc_max_pos_int; + rcl_interfaces::msg::FloatingPointRange r_max_pos_int; + r_max_pos_int.set__from_value(0.0).set__to_value(4.00).set__step(0); + desc_max_pos_int.type = 3; // PARAMETER_DOUBLE + desc_max_pos_int.description = std::string("World max integral"); + desc_max_pos_int.floating_point_range = {r_max_pos_int}; + this->declare_parameter("max_pos_int", 0.5f, desc_max_pos_int); + + rcl_interfaces::msg::ParameterDescriptor desc_max_pos_int_b; + rcl_interfaces::msg::FloatingPointRange r_max_pos_int_b; + r_max_pos_int_b.set__from_value(0.0).set__to_value(4.00).set__step(0); + desc_max_pos_int_b.type = 3; // PARAMETER_DOUBLE + desc_max_pos_int_b.description = std::string("Body max integral"); + desc_max_pos_int_b.floating_point_range = {r_max_pos_int_b}; + this->declare_parameter("max_pos_int_b", 0.5f, desc_max_pos_int_b); + + rcl_interfaces::msg::ParameterDescriptor desc_max_tilt_angle; + rcl_interfaces::msg::FloatingPointRange r_max_tilt_angle; + r_max_tilt_angle.set__from_value(0.0).set__to_value(static_cast(M_PI)).set__step(0); + desc_max_tilt_angle.type = 3; // PARAMETER_DOUBLE + desc_max_tilt_angle.description = std::string("Max tilt angle"); + desc_max_tilt_angle.floating_point_range = {r_max_tilt_angle}; + this->declare_parameter("max_tilt_angle", 3.14f, desc_max_tilt_angle); + + // Getting dynamic parameters + this->get_parameter("kp_x", config_kx_[0]); + this->get_parameter("kp_y", config_kx_[1]); + this->get_parameter("kp_z", config_kx_[2]); + kx_[0] = config_kx_[0]; + kx_[1] = config_kx_[1]; + kx_[2] = config_kx_[2]; + + this->get_parameter("kd_x", config_kv_[0]); + this->get_parameter("kd_y", config_kv_[1]); + this->get_parameter("kd_z", config_kv_[2]); + kv_[0] = config_kv_[0]; + kv_[1] = config_kv_[1]; + kv_[2] = config_kv_[2]; + + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + + this->get_parameter("ki_x", config_ki_[0]); + this->get_parameter("ki_y", config_ki_[1]); + this->get_parameter("ki_z", config_ki_[2]); + + this->get_parameter("kib_x", config_kib_[0]); + this->get_parameter("kib_y", config_kib_[1]); + this->get_parameter("kib_z", config_kib_[2]); + + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + + this->get_parameter("rot_x", kr_[0]); + this->get_parameter("rot_y", kr_[1]); + this->get_parameter("rot_z", kr_[2]); + + this->get_parameter("ang_x", kom_[0]); + this->get_parameter("ang_y", kom_[1]); + this->get_parameter("ang_z", kom_[2]); + + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", kr_[0], + kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + + this->get_parameter("kf_correction", corrections_[0]); + this->get_parameter("roll_correction", corrections_[1]); + this->get_parameter("pitch_correction", corrections_[2]); + + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + + float max_pos_int, max_pos_int_b; + this->get_parameter("max_pos_int", max_pos_int); + this->get_parameter("max_pos_int_b", max_pos_int_b); + controller_.setMaxIntegral(max_pos_int); + controller_.setMaxIntegralBody(max_pos_int_b); + + float max_tilt_angle; + this->get_parameter("max_tilt_angle", max_tilt_angle); + controller_.setMaxTiltAngle(max_tilt_angle); + + RCLCPP_INFO(this->get_logger(), "Maxes set to Integral: %2.2g, Integral Body: %2.2g, Tilt Angle (Rad): %2.2g", + max_pos_int, max_pos_int_b, max_tilt_angle); + + // Initialize dynamic reconfigure callback + reconfigure_handle_ = + this->add_on_set_parameters_callback(std::bind(&SO3ControlComponent::cfg_callback, this, std::placeholders::_1)); + + so3_command_pub_ = this->create_publisher("~/so3_cmd", 10); + command_viz_pub_ = this->create_publisher("~/cmd_viz", 10); + + // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos1 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 2), qos_profile); + + odom_sub_ = this->create_subscription( + "~/odom", qos1, std::bind(&SO3ControlComponent::odom_callback, this, std::placeholders::_1)); + position_cmd_sub_ = this->create_subscription( + "~/position_cmd", qos1, std::bind(&SO3ControlComponent::position_cmd_callback, this, std::placeholders::_1)); + enable_motors_sub_ = this->create_subscription( + "~/motors", qos2, std::bind(&SO3ControlComponent::enable_motors_callback, this, std::placeholders::_1)); + corrections_sub_ = this->create_subscription( + "~/corrections", qos1, std::bind(&SO3ControlComponent::corrections_callback, this, std::placeholders::_1)); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(SO3ControlComponent) diff --git a/kr_mav_controllers/src/so3_control_nodelet.cpp b/kr_mav_controllers/src/so3_control_nodelet.cpp deleted file mode 100644 index 38ef3e56..00000000 --- a/kr_mav_controllers/src/so3_control_nodelet.cpp +++ /dev/null @@ -1,376 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -class SO3ControlNodelet : public nodelet::Nodelet -{ - public: - SO3ControlNodelet() - : position_cmd_updated_(false), - position_cmd_init_(false), - des_yaw_(0), - des_yaw_dot_(0), - current_yaw_(0), - enable_motors_(false), - use_external_yaw_(false), - have_odom_(false), - g_(9.81), - current_orientation_(Eigen::Quaternionf::Identity()) - { - controller_.resetIntegrals(); - } - - void onInit(); - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // Need this since we have SO3Control which needs aligned pointer - - private: - void publishSO3Command(); - void position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - void enable_motors_callback(const std_msgs::Bool::ConstPtr &msg); - void corrections_callback(const kr_mav_msgs::Corrections::ConstPtr &msg); - void cfg_callback(kr_mav_controllers::SO3Config &config, uint32_t level); - - SO3Control controller_; - ros::Publisher so3_command_pub_, command_viz_pub_; - ros::Subscriber odom_sub_, position_cmd_sub_, enable_motors_sub_, corrections_sub_; - - bool position_cmd_updated_, position_cmd_init_; - std::string frame_id_; - - Eigen::Vector3f des_pos_, des_vel_, des_acc_, des_jrk_, config_kx_, config_kv_, config_ki_, config_kib_, kx_, kv_; - float des_yaw_, des_yaw_dot_; - float current_yaw_; - bool enable_motors_, use_external_yaw_, have_odom_; - float kR_[3], kOm_[3], corrections_[3]; - float mass_; - const float g_; - Eigen::Quaternionf current_orientation_; - - boost::recursive_mutex config_mutex_; - typedef dynamic_reconfigure::Server ReconfigureServer; - boost::shared_ptr reconfigure_server_; -}; - -void SO3ControlNodelet::publishSO3Command() -{ - if(!have_odom_) - { - ROS_WARN("No odometry! Not publishing SO3Command."); - return; - } - - Eigen::Vector3f ki = Eigen::Vector3f::Zero(); - Eigen::Vector3f kib = Eigen::Vector3f::Zero(); - if(enable_motors_) - { - ki = config_ki_; - kib = config_kib_; - } - - controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_jrk_, des_yaw_, des_yaw_dot_, kx_, kv_, ki, kib); - - const Eigen::Vector3f &force = controller_.getComputedForce(); - const Eigen::Quaternionf &orientation = controller_.getComputedOrientation(); - const Eigen::Vector3f &ang_vel = controller_.getComputedAngularVelocity(); - - kr_mav_msgs::SO3Command::Ptr so3_command = boost::make_shared(); - so3_command->header.stamp = ros::Time::now(); - so3_command->header.frame_id = frame_id_; - so3_command->force.x = force(0); - so3_command->force.y = force(1); - so3_command->force.z = force(2); - so3_command->orientation.x = orientation.x(); - so3_command->orientation.y = orientation.y(); - so3_command->orientation.z = orientation.z(); - so3_command->orientation.w = orientation.w(); - so3_command->angular_velocity.x = ang_vel(0); - so3_command->angular_velocity.y = ang_vel(1); - so3_command->angular_velocity.z = ang_vel(2); - for(int i = 0; i < 3; i++) - { - so3_command->kR[i] = kR_[i]; - so3_command->kOm[i] = kOm_[i]; - } - so3_command->aux.current_yaw = current_yaw_; - so3_command->aux.kf_correction = corrections_[0]; - so3_command->aux.angle_corrections[0] = corrections_[1]; - so3_command->aux.angle_corrections[1] = corrections_[2]; - so3_command->aux.enable_motors = enable_motors_; - so3_command->aux.use_external_yaw = use_external_yaw_; - so3_command_pub_.publish(so3_command); - - geometry_msgs::PoseStamped::Ptr cmd_viz_msg = boost::make_shared(); - cmd_viz_msg->header = so3_command->header; - cmd_viz_msg->pose.position.x = des_pos_(0); - cmd_viz_msg->pose.position.y = des_pos_(1); - cmd_viz_msg->pose.position.z = des_pos_(2); - cmd_viz_msg->pose.orientation.x = orientation.x(); - cmd_viz_msg->pose.orientation.y = orientation.y(); - cmd_viz_msg->pose.orientation.z = orientation.z(); - cmd_viz_msg->pose.orientation.w = orientation.w(); - command_viz_pub_.publish(cmd_viz_msg); -} - -void SO3ControlNodelet::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) -{ - des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z); - des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); - des_acc_ = Eigen::Vector3f(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z); - des_jrk_ = Eigen::Vector3f(cmd->jerk.x, cmd->jerk.y, cmd->jerk.z); - - // Check use_msg_gains_flag to decide whether to use gains from the msg or config - kx_[0] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_X) ? cmd->kx[0] : config_kx_[0]; - kx_[1] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_Y) ? cmd->kx[1] : config_kx_[1]; - kx_[2] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_Z) ? cmd->kx[2] : config_kx_[2]; - kv_[0] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_X) ? cmd->kv[0] : config_kv_[0]; - kv_[1] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_Y) ? cmd->kv[1] : config_kv_[1]; - kv_[2] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_Z) ? cmd->kv[2] : config_kv_[2]; - - des_yaw_ = cmd->yaw; - des_yaw_dot_ = cmd->yaw_dot; - position_cmd_updated_ = true; - // position_cmd_init_ = true; - - publishSO3Command(); -} - -void SO3ControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) -{ - have_odom_ = true; - - frame_id_ = odom->header.frame_id; - - const Eigen::Vector3f position(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z); - const Eigen::Vector3f velocity(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z); - - current_yaw_ = tf::getYaw(odom->pose.pose.orientation); - - current_orientation_ = Eigen::Quaternionf(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, - odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); - - controller_.setPosition(position); - controller_.setVelocity(velocity); - controller_.setCurrentOrientation(current_orientation_); - - if(position_cmd_init_) - { - // We set position_cmd_updated_ = false and expect that the - // position_cmd_callback would set it to true since typically a position_cmd - // message would follow an odom message. If not, the position_cmd_callback - // hasn't been called and we publish the so3 command ourselves - // TODO: Fallback to hover if position_cmd hasn't been received for some time - if(!position_cmd_updated_) - publishSO3Command(); - position_cmd_updated_ = false; - } -} - -void SO3ControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr &msg) -{ - if(msg->data) - ROS_INFO("Enabling motors"); - else - ROS_INFO("Disabling motors"); - - enable_motors_ = msg->data; - // Reset integral when toggling motor state - controller_.resetIntegrals(); -} - -void SO3ControlNodelet::corrections_callback(const kr_mav_msgs::Corrections::ConstPtr &msg) -{ - corrections_[0] = msg->kf_correction; - corrections_[1] = msg->angle_corrections[0]; - corrections_[2] = msg->angle_corrections[1]; -} - -void SO3ControlNodelet::cfg_callback(kr_mav_controllers::SO3Config &config, uint32_t level) -{ - if(level == 0) - { - NODELET_DEBUG_STREAM("Nothing changed. level: " << level); - return; - } - - if(level & (1 << 0)) - { - config_kx_[0] = config.kp_x; - config_kx_[1] = config.kp_y; - config_kx_[2] = config.kp_z; - - config_kv_[0] = config.kd_x; - config_kv_[1] = config.kd_y; - config_kv_[2] = config.kd_z; - - NODELET_INFO("Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", config_kx_[0], - config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); - } - - if(level & (1 << 1)) - { - config_ki_[0] = config.ki_x; - config_ki_[1] = config.ki_y; - config_ki_[2] = config.ki_z; - - config_kib_[0] = config.kib_x; - config_kib_[1] = config.kib_y; - config_kib_[2] = config.kib_z; - - NODELET_INFO("Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", config_ki_[0], - config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); - } - - if(level & (1 << 2)) - { - kR_[0] = config.rot_x; - kR_[1] = config.rot_y; - kR_[2] = config.rot_z; - - kOm_[0] = config.ang_x; - kOm_[1] = config.ang_y; - kOm_[2] = config.ang_z; - - NODELET_INFO("Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", kR_[0], kR_[1], kR_[2], - kOm_[0], kOm_[1], kOm_[2]); - } - - if(level & (1 << 3)) - { - corrections_[0] = config.kf_correction; - corrections_[1] = config.roll_correction; - corrections_[2] = config.pitch_correction; - NODELET_INFO("Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], corrections_[1], - corrections_[2]); - } - - if(level & (1 << 4)) - { - controller_.setMaxIntegral(config.max_pos_int); - controller_.setMaxIntegralBody(config.max_pos_int_b); - controller_.setMaxTiltAngle(config.max_tilt_angle); - - NODELET_INFO("Maxes set to Integral: %2.2g, Integral Body: %2.2g, Tilt Angle (rad): %2.2g", config.max_pos_int, - config.max_pos_int_b, config.max_tilt_angle); - } - - NODELET_WARN_STREAM_COND(level != std::numeric_limits::max() && (level >= (1 << 5)), - "kr_mav_controllers dynamic reconfigure called, but with unknown level: " << level); -} - -void SO3ControlNodelet::onInit(void) -{ - ros::NodeHandle priv_nh(getPrivateNodeHandle()); - - std::string quadrotor_name; - priv_nh.param("quadrotor_name", quadrotor_name, std::string("quadrotor")); - frame_id_ = "/" + quadrotor_name; - - priv_nh.param("mass", mass_, 0.5f); - controller_.setMass(mass_); - controller_.setGravity(g_); - - // Dynamic reconfigure struct - kr_mav_controllers::SO3Config config; - - priv_nh.param("use_external_yaw", use_external_yaw_, true); - - priv_nh.param("gains/pos/x", config_kx_[0], 7.4f); - priv_nh.param("gains/pos/y", config_kx_[1], 7.4f); - priv_nh.param("gains/pos/z", config_kx_[2], 10.4f); - kx_[0] = config_kx_[0]; - kx_[1] = config_kx_[1]; - kx_[2] = config_kx_[2]; - config.kp_x = config_kx_[0]; - config.kp_y = config_kx_[1]; - config.kp_z = config_kx_[2]; - - priv_nh.param("gains/vel/x", config_kv_[0], 4.8f); - priv_nh.param("gains/vel/y", config_kv_[1], 4.8f); - priv_nh.param("gains/vel/z", config_kv_[2], 6.0f); - kv_[0] = config_kv_[0]; - kv_[1] = config_kv_[1]; - kv_[2] = config_kv_[2]; - config.kd_x = config_kv_[0]; - config.kd_y = config_kv_[1]; - config.kd_z = config_kv_[2]; - - priv_nh.param("gains/ki/x", config_ki_[0], 0.0f); - priv_nh.param("gains/ki/y", config_ki_[1], 0.0f); - priv_nh.param("gains/ki/z", config_ki_[2], 0.0f); - config.ki_x = config_ki_[0]; - config.ki_y = config_ki_[1]; - config.ki_z = config_ki_[2]; - - priv_nh.param("gains/kib/x", config_kib_[0], 0.0f); - priv_nh.param("gains/kib/y", config_kib_[1], 0.0f); - priv_nh.param("gains/kib/z", config_kib_[2], 0.0f); - config.kib_x = config_kib_[0]; - config.kib_y = config_kib_[1]; - config.kib_z = config_kib_[2]; - - priv_nh.param("gains/rot/x", kR_[0], 1.5f); - priv_nh.param("gains/rot/y", kR_[1], 1.5f); - priv_nh.param("gains/rot/z", kR_[2], 1.0f); - priv_nh.param("gains/ang/x", kOm_[0], 0.13f); - priv_nh.param("gains/ang/y", kOm_[1], 0.13f); - priv_nh.param("gains/ang/z", kOm_[2], 0.1f); - config.rot_x = kR_[0]; - config.rot_y = kR_[1]; - config.rot_z = kR_[2]; - config.ang_x = kOm_[0]; - config.ang_y = kOm_[1]; - config.ang_z = kOm_[2]; - - priv_nh.param("corrections/kf", corrections_[0], 0.0f); - priv_nh.param("corrections/r", corrections_[1], 0.0f); - priv_nh.param("corrections/p", corrections_[2], 0.0f); - config.kf_correction = corrections_[0]; - config.roll_correction = corrections_[1]; - config.pitch_correction = corrections_[2]; - - float max_pos_int, max_pos_int_b; - priv_nh.param("max_pos_int", max_pos_int, 0.5f); - priv_nh.param("mas_pos_int_b", max_pos_int_b, 0.5f); - controller_.setMaxIntegral(max_pos_int); - controller_.setMaxIntegralBody(max_pos_int_b); - config.max_pos_int = max_pos_int; - config.max_pos_int_b = max_pos_int_b; - - float max_tilt_angle; - priv_nh.param("max_tilt_angle", max_tilt_angle, static_cast(M_PI)); - controller_.setMaxTiltAngle(max_tilt_angle); - config.max_tilt_angle = max_tilt_angle; - - // Initialize dynamic reconfigure - reconfigure_server_ = boost::make_shared(config_mutex_, priv_nh); - reconfigure_server_->updateConfig(config); - reconfigure_server_->setCallback(boost::bind(&SO3ControlNodelet::cfg_callback, this, _1, _2)); - - so3_command_pub_ = priv_nh.advertise("so3_cmd", 10); - command_viz_pub_ = priv_nh.advertise("cmd_viz", 10); - - odom_sub_ = - priv_nh.subscribe("odom", 10, &SO3ControlNodelet::odom_callback, this, ros::TransportHints().tcpNoDelay()); - position_cmd_sub_ = priv_nh.subscribe("position_cmd", 10, &SO3ControlNodelet::position_cmd_callback, this, - ros::TransportHints().tcpNoDelay()); - enable_motors_sub_ = priv_nh.subscribe("motors", 2, &SO3ControlNodelet::enable_motors_callback, this, - ros::TransportHints().tcpNoDelay()); - corrections_sub_ = priv_nh.subscribe("corrections", 10, &SO3ControlNodelet::corrections_callback, this, - ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(SO3ControlNodelet, nodelet::Nodelet); diff --git a/kr_mav_controllers/src/so3_trpy_control.cpp b/kr_mav_controllers/src/so3_trpy_control.cpp index 3ca01d50..3c4b750f 100644 --- a/kr_mav_controllers/src/so3_trpy_control.cpp +++ b/kr_mav_controllers/src/so3_trpy_control.cpp @@ -1,73 +1,56 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include +#include "kr_mav_controllers/SO3Control.hpp" +#include "kr_mav_msgs/msg/corrections.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_mav_msgs/msg/trpy_command.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" + #define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x) -class SO3TRPYControlNodelet : public nodelet::Nodelet +class SO3TRPYControlComponent : public rclcpp::Node { public: - SO3TRPYControlNodelet() - : odom_set_(false), - position_cmd_updated_(false), - position_cmd_init_(false), - des_yaw_(0), - des_yaw_dot_(0), - yaw_int_(0), - enable_motors_(false), - use_external_yaw_(false), - g_(9.81) - { - controller_.resetIntegrals(); - } - - void onInit(); - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // Need this since we have SO3Control which needs aligned pointer + SO3TRPYControlComponent(const rclcpp::NodeOptions &options); private: void publishCommand(); - void position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - void enable_motors_callback(const std_msgs::Bool::ConstPtr &msg); - void corrections_callback(const kr_mav_msgs::Corrections::ConstPtr &msg); - void cfg_callback(kr_mav_controllers::SO3Config &config, uint32_t level); + void position_cmd_callback(const kr_mav_msgs::msg::PositionCommand::UniquePtr cmd); + void odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom); + void enable_motors_callback(const std_msgs::msg::Bool::UniquePtr msg); + void corrections_callback(const kr_mav_msgs::msg::Corrections::UniquePtr msg); + rcl_interfaces::msg::SetParametersResult cfg_callback(std::vector parameters); SO3Control controller_; - ros::Publisher trpy_command_pub_; - ros::Subscriber odom_sub_, position_cmd_sub_, enable_motors_sub_, corrections_sub_; + rclcpp::Publisher::SharedPtr trpy_command_pub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr position_cmd_sub_; + rclcpp::Subscription::SharedPtr enable_motors_sub_; + rclcpp::Subscription::SharedPtr corrections_sub_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfigure_handle_; bool odom_set_, position_cmd_updated_, position_cmd_init_; std::string frame_id_; - Eigen::Vector3f des_pos_, des_vel_, des_acc_, des_jrk_, config_kx_, config_kv_, config_ki_, config_kib_, kx_, kv_; + Eigen::Vector3f des_pos_, des_vel_, des_acc_, des_jrk_, kx_, kv_, config_kx_, config_kv_, config_ki_, config_kib_; float des_yaw_, des_yaw_dot_, yaw_int_; bool enable_motors_, use_external_yaw_; - float kR_[3], kOm_[3], corrections_[3]; + float kr_[3], kom_[3], corrections_[3]; float ki_yaw_; float mass_; const float g_; Eigen::Quaternionf current_orientation_; - boost::recursive_mutex config_mutex_; - typedef dynamic_reconfigure::Server ReconfigureServer; - boost::shared_ptr reconfigure_server_; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Need this since we have SO3Control which needs aligned pointer }; -void SO3TRPYControlNodelet::publishCommand() +void SO3TRPYControlComponent::publishCommand() { if(!odom_set_) { - NODELET_INFO_THROTTLE(1, "Odom not set, not publishing command!"); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Odom no set, not publishing command!"); return; } @@ -123,8 +106,8 @@ void SO3TRPYControlNodelet::publishCommand() else if(yaw_cmd < -PI) yaw_cmd += 2 * PI; - kr_mav_msgs::TRPYCommand::Ptr trpy_command(new kr_mav_msgs::TRPYCommand); - trpy_command->header.stamp = ros::Time::now(); + auto trpy_command = std::make_unique(); + trpy_command->header.stamp = this->now(); trpy_command->header.frame_id = frame_id_; if(enable_motors_) { @@ -137,8 +120,8 @@ void SO3TRPYControlNodelet::publishCommand() trpy_command->angular_velocity.z = ang_vel(2); for(int i = 0; i < 3; i++) { - trpy_command->kR[i] = kR_[i]; - trpy_command->kOm[i] = kOm_[i]; + trpy_command->kr[i] = kr_[i]; + trpy_command->kom[i] = kom_[i]; } } trpy_command->aux.current_yaw = yaw_cur; @@ -147,10 +130,10 @@ void SO3TRPYControlNodelet::publishCommand() trpy_command->aux.angle_corrections[1] = corrections_[2]; trpy_command->aux.enable_motors = enable_motors_; trpy_command->aux.use_external_yaw = use_external_yaw_; - trpy_command_pub_.publish(trpy_command); + trpy_command_pub_->publish(std::move(trpy_command)); } -void SO3TRPYControlNodelet::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) +void SO3TRPYControlComponent::position_cmd_callback(const kr_mav_msgs::msg::PositionCommand::UniquePtr cmd) { des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z); des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); @@ -173,7 +156,7 @@ void SO3TRPYControlNodelet::position_cmd_callback(const kr_mav_msgs::PositionCom publishCommand(); } -void SO3TRPYControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) +void SO3TRPYControlComponent::odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom) { if(!odom_set_) odom_set_ = true; @@ -201,12 +184,12 @@ void SO3TRPYControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr &od } } -void SO3TRPYControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr &msg) +void SO3TRPYControlComponent::enable_motors_callback(const std_msgs::msg::Bool::UniquePtr msg) { if(msg->data) - ROS_INFO("Enabling motors"); + RCLCPP_INFO(this->get_logger(), "Enabling motors"); else - ROS_INFO("Disabling motors"); + RCLCPP_INFO(this->get_logger(), "Disabling motors"); enable_motors_ = msg->data; @@ -215,189 +198,458 @@ void SO3TRPYControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPt controller_.resetIntegrals(); } -void SO3TRPYControlNodelet::corrections_callback(const kr_mav_msgs::Corrections::ConstPtr &msg) +void SO3TRPYControlComponent::corrections_callback(const kr_mav_msgs::msg::Corrections::UniquePtr msg) { corrections_[0] = msg->kf_correction; corrections_[1] = msg->angle_corrections[0]; corrections_[2] = msg->angle_corrections[1]; } - -void SO3TRPYControlNodelet::cfg_callback(kr_mav_controllers::SO3Config &config, uint32_t level) +rcl_interfaces::msg::SetParametersResult SO3TRPYControlComponent::cfg_callback( + std::vector parameters) { - if(level == 0) - { - NODELET_DEBUG_STREAM("Nothing changed. level: " << level); - return; - } - - if(level & (1 << 0)) + rcl_interfaces::msg::SetParametersResult result; + for(auto ¶meter : parameters) { - config_kx_[0] = config.kp_x; - config_kx_[1] = config.kp_y; - config_kx_[2] = config.kp_z; - - config_kv_[0] = config.kd_x; - config_kv_[1] = config.kd_y; - config_kv_[2] = config.kd_z; + const auto &name = parameter.get_name(); - NODELET_INFO("Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", config_kx_[0], - config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); - } - - if(level & (1 << 1)) - { - config_ki_[0] = config.ki_x; - config_ki_[1] = config.ki_y; - config_ki_[2] = config.ki_z; - - config_kib_[0] = config.kib_x; - config_kib_[1] = config.kib_y; - config_kib_[2] = config.kib_z; - - NODELET_INFO("Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", config_ki_[0], - config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); - } - - if(level & (1 << 2)) - { - kR_[0] = config.rot_x; - kR_[1] = config.rot_y; - kR_[2] = config.rot_z; - - kOm_[0] = config.ang_x; - kOm_[1] = config.ang_y; - kOm_[2] = config.ang_z; - - NODELET_INFO("Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", kR_[0], kR_[1], kR_[2], - kOm_[0], kOm_[1], kOm_[2]); - } - - if(level & (1 << 3)) - { - corrections_[0] = config.kf_correction; - corrections_[1] = config.roll_correction; - corrections_[2] = config.pitch_correction; - NODELET_INFO("Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], corrections_[1], - corrections_[2]); - } - - if(level & (1 << 4)) - { - controller_.setMaxIntegral(config.max_pos_int); - controller_.setMaxIntegralBody(config.max_pos_int_b); - controller_.setMaxTiltAngle(config.max_tilt_angle); - - NODELET_INFO("Maxes set to Integral: %2.2g, Integral Body: %2.2g, Tilt Angle (rad): %2.2g", config.max_pos_int, - config.max_pos_int_b, config.max_tilt_angle); + if(name == "kp_x") + { + config_kx_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kp_y") + { + config_kx_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kp_z") + { + config_kx_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kd_x") + { + config_kv_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kd_y") + { + config_kv_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kd_z") + { + config_kv_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "ki_x") + { + config_ki_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_y") + { + config_ki_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_z") + { + config_ki_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "kib_x") + { + config_kib_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_y") + { + config_kib_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_z") + { + config_kib_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "rot_x") + { + kr_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "rot_y") + { + kr_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "rot_z") + { + kr_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "ang_x") + { + kom_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "ang_y") + { + kom_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "ang_z") + { + kom_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "kf_correction") + { + corrections_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + } + else if(name == "roll_correction") + { + corrections_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + } + else if(name == "pitch_correction") + { + corrections_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + } + else if(name == "max_pos_int") + { + controller_.setMaxIntegral(parameter.as_double()); + RCLCPP_INFO(this->get_logger(), "World max integral set to: %2.2g", parameter.as_double()); + } + else if(name == "max_pos_int_b") + { + controller_.setMaxIntegralBody(parameter.as_double()); + RCLCPP_INFO(this->get_logger(), "Body max integral set to: %2.2g", parameter.as_double()); + } + else if(name == "max_tilt_angle") + { + controller_.setMaxTiltAngle(parameter.as_double()); + RCLCPP_INFO(this->get_logger(), "Max tilt angle set to: %2.2g", parameter.as_double()); + } + else + { + RCLCPP_WARN_STREAM(this->get_logger(), "kr_mav_controllers dynamic reconfigure called with invalid parameter"); + result.successful = false; + return result; + } } - - NODELET_WARN_STREAM_COND(level != std::numeric_limits::max() && (level >= (1 << 5)), - "kr_mav_controllers dynamic reconfigure called, but with unknown level: " << level); + result.successful = true; + return result; } -void SO3TRPYControlNodelet::onInit() +SO3TRPYControlComponent::SO3TRPYControlComponent(const rclcpp::NodeOptions &options) + : Node("so3_control", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + odom_set_(false), + position_cmd_updated_(false), + position_cmd_init_(false), + des_yaw_(0), + des_yaw_dot_(0), + yaw_int_(0), + enable_motors_(false), + use_external_yaw_(false), + g_(9.81) { - ros::NodeHandle n(getPrivateNodeHandle()); + controller_.resetIntegrals(); + + this->declare_parameter("quadrotor_name", std::string("quadrotor")); + this->declare_parameter("mass", 0.5f); std::string quadrotor_name; - n.param("quadrotor_name", quadrotor_name, std::string("quadrotor")); + this->get_parameter("quadrotor_name", quadrotor_name); frame_id_ = "/" + quadrotor_name; + this->get_parameter("mass", mass_); - float mass; - n.param("mass", mass, 0.5f); - mass_ = mass; controller_.setMass(mass_); controller_.setGravity(g_); - // Dynamic reconfigure struct - kr_mav_controllers::SO3Config config; - - n.param("use_external_yaw", use_external_yaw_, true); - - n.param("gains/pos/x", config_kx_[0], 7.4f); - n.param("gains/pos/y", config_kx_[1], 7.4f); - n.param("gains/pos/z", config_kx_[2], 10.4f); + this->declare_parameter("use_external_yaw", true); + this->get_parameter("use_external_yaw", use_external_yaw_); + + // Dynamic parameter reconfiguration definitions + + // Translation Gains: World Position Gains + rcl_interfaces::msg::ParameterDescriptor desc_kp_x; + rcl_interfaces::msg::FloatingPointRange r_kp_x; + r_kp_x.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kp_x.type = 3; // PARAMETER_DOUBLE + desc_kp_x.description = std::string("World x position gain"); + desc_kp_x.floating_point_range = {r_kp_x}; + this->declare_parameter("kp_x", 7.4f, desc_kp_x); + + rcl_interfaces::msg::ParameterDescriptor desc_kp_y; + rcl_interfaces::msg::FloatingPointRange r_kp_y; + r_kp_y.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kp_y.type = 3; // PARAMETER_DOUBLE + desc_kp_y.description = std::string("World y position gain"); + desc_kp_y.floating_point_range = {r_kp_y}; + this->declare_parameter("kp_y", 7.4f, desc_kp_y); + + rcl_interfaces::msg::ParameterDescriptor desc_kp_z; + rcl_interfaces::msg::FloatingPointRange r_kp_z; + r_kp_z.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kp_z.type = 3; // PARAMETER_DOUBLE + desc_kp_z.description = std::string("World z position gain"); + desc_kp_z.floating_point_range = {r_kp_z}; + this->declare_parameter("kp_z", 10.4f, desc_kp_z); + + // Translation Gains: World Derivative Gains + rcl_interfaces::msg::ParameterDescriptor desc_kd_x; + rcl_interfaces::msg::FloatingPointRange r_kd_x; + r_kd_x.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kd_x.type = 3; // PARAMETER_DOUBLE + desc_kd_x.description = std::string("World x derivative gain"); + desc_kd_x.floating_point_range = {r_kd_x}; + this->declare_parameter("kd_x", 4.8f, desc_kd_x); + + rcl_interfaces::msg::ParameterDescriptor desc_kd_y; + rcl_interfaces::msg::FloatingPointRange r_kd_y; + r_kd_y.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kd_y.type = 3; // PARAMETER_DOUBLE + desc_kd_y.description = std::string("World y derivative gain"); + desc_kd_y.floating_point_range = {r_kd_y}; + this->declare_parameter("kd_y", 4.8f, desc_kd_y); + + rcl_interfaces::msg::ParameterDescriptor desc_kd_z; + rcl_interfaces::msg::FloatingPointRange r_kd_z; + r_kd_z.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kd_z.type = 3; // PARAMETER_DOUBLE + desc_kd_z.description = std::string("World z derivative gain"); + desc_kd_z.floating_point_range = {r_kd_z}; + this->declare_parameter("kd_z", 6.0f, desc_kd_z); + + // Integral Gains: World Integral Gains + rcl_interfaces::msg::ParameterDescriptor desc_ki_x; + rcl_interfaces::msg::FloatingPointRange r_ki_x; + r_ki_x.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_ki_x.type = 3; // PARAMETER_DOUBLE + desc_ki_x.description = std::string("World x integral gain"); + desc_ki_x.floating_point_range = {r_ki_x}; + this->declare_parameter("ki_x", 0.0f, desc_ki_x); + + rcl_interfaces::msg::ParameterDescriptor desc_ki_y; + rcl_interfaces::msg::FloatingPointRange r_ki_y; + r_ki_y.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_ki_y.type = 3; // PARAMETER_DOUBLE + desc_ki_y.description = std::string("World y integral gain"); + desc_ki_y.floating_point_range = {r_ki_y}; + this->declare_parameter("ki_y", 0.0f, desc_ki_y); + + rcl_interfaces::msg::ParameterDescriptor desc_ki_z; + rcl_interfaces::msg::FloatingPointRange r_ki_z; + r_ki_z.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_ki_z.type = 3; // PARAMETER_DOUBLE + desc_ki_z.description = std::string("World z integral gain"); + desc_ki_z.floating_point_range = {r_ki_z}; + this->declare_parameter("ki_z", 0.0f, desc_ki_z); + + // Integral Gains: Body Integral Gains + rcl_interfaces::msg::ParameterDescriptor desc_kib_x; + rcl_interfaces::msg::FloatingPointRange r_kib_x; + r_kib_x.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_kib_x.type = 3; // PARAMETER_DOUBLE + desc_kib_x.description = std::string("Body x integral gain"); + desc_kib_x.floating_point_range = {r_kib_x}; + this->declare_parameter("kib_x", 0.0f, desc_kib_x); + + rcl_interfaces::msg::ParameterDescriptor desc_kib_y; + rcl_interfaces::msg::FloatingPointRange r_kib_y; + r_kib_y.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_kib_y.type = 3; // PARAMETER_DOUBLE + desc_kib_y.description = std::string("Body y integral gain"); + desc_kib_y.floating_point_range = {r_kib_y}; + this->declare_parameter("kib_y", 0.0f, desc_kib_y); + + rcl_interfaces::msg::ParameterDescriptor desc_kib_z; + rcl_interfaces::msg::FloatingPointRange r_kib_z; + r_kib_z.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_kib_z.type = 3; // PARAMETER_DOUBLE + desc_kib_z.description = std::string("Body z integral gain"); + desc_kib_z.floating_point_range = {r_kib_z}; + this->declare_parameter("kib_z", 0.0f, desc_kib_z); + + // Attitude Gains: Rotation Gains + rcl_interfaces::msg::ParameterDescriptor desc_rot_x; + rcl_interfaces::msg::FloatingPointRange r_rot_x; + r_rot_x.set__from_value(0.0).set__to_value(3.00).set__step(0); + desc_rot_x.type = 3; // PARAMETER_DOUBLE + desc_rot_x.description = std::string("Rotation x gain"); + desc_rot_x.floating_point_range = {r_rot_x}; + this->declare_parameter("rot_x", 1.5f, desc_rot_x); + + rcl_interfaces::msg::ParameterDescriptor desc_rot_y; + rcl_interfaces::msg::FloatingPointRange r_rot_y; + r_rot_y.set__from_value(0.0).set__to_value(3.00).set__step(0); + desc_rot_y.type = 3; // PARAMETER_DOUBLE + desc_rot_y.description = std::string("Rotation y gain"); + desc_rot_y.floating_point_range = {r_rot_y}; + this->declare_parameter("rot_y", 1.5f, desc_rot_y); + + rcl_interfaces::msg::ParameterDescriptor desc_rot_z; + rcl_interfaces::msg::FloatingPointRange r_rot_z; + r_rot_z.set__from_value(0.0).set__to_value(3.00).set__step(0); + desc_rot_z.type = 3; // PARAMETER_DOUBLE + desc_rot_z.description = std::string("Rotation z gain"); + desc_rot_z.floating_point_range = {r_rot_z}; + this->declare_parameter("rot_z", 1.0f, desc_rot_z); + + // Attitude Gains: Angular Gains + rcl_interfaces::msg::ParameterDescriptor desc_ang_x; + rcl_interfaces::msg::FloatingPointRange r_ang_x; + r_ang_x.set__from_value(0.0).set__to_value(1.00).set__step(0); + desc_ang_x.type = 3; // PARAMETER_DOUBLE + desc_ang_x.description = std::string("Angular x gain"); + desc_ang_x.floating_point_range = {r_ang_x}; + this->declare_parameter("ang_x", 0.13f, desc_ang_x); + + rcl_interfaces::msg::ParameterDescriptor desc_ang_y; + rcl_interfaces::msg::FloatingPointRange r_ang_y; + r_ang_y.set__from_value(0.0).set__to_value(1.00).set__step(0); + desc_ang_y.type = 3; // PARAMETER_DOUBLE + desc_ang_y.description = std::string("Angular y gain"); + desc_ang_y.floating_point_range = {r_ang_y}; + this->declare_parameter("ang_y", 0.13f, desc_ang_y); + + rcl_interfaces::msg::ParameterDescriptor desc_ang_z; + rcl_interfaces::msg::FloatingPointRange r_ang_z; + r_ang_z.set__from_value(0.0).set__to_value(1.00).set__step(0); + desc_ang_z.type = 3; // PARAMETER_DOUBLE + desc_ang_z.description = std::string("Angular z gain"); + desc_ang_z.floating_point_range = {r_ang_z}; + this->declare_parameter("ang_z", 0.10f, desc_ang_z); + + // Low level Corrections + rcl_interfaces::msg::ParameterDescriptor desc_kf_correction; + desc_kf_correction.type = 3; // PARAMETER_DOUBLE + desc_kf_correction.description = std::string("kf"); + this->declare_parameter("kf_correction", 0.0f, desc_kf_correction); + + rcl_interfaces::msg::ParameterDescriptor desc_roll_correction; + desc_roll_correction.type = 3; // PARAMETER_DOUBLE + desc_roll_correction.description = std::string("roll"); + this->declare_parameter("roll_correction", 0.0f, desc_roll_correction); + + rcl_interfaces::msg::ParameterDescriptor desc_pitch_correction; + desc_pitch_correction.type = 3; // PARAMETER_DOUBLE + desc_pitch_correction.description = std::string("pitch"); + this->declare_parameter("pitch_correction", 0.0f, desc_pitch_correction); + + // Misc Parameters + rcl_interfaces::msg::ParameterDescriptor desc_max_pos_int; + rcl_interfaces::msg::FloatingPointRange r_max_pos_int; + r_max_pos_int.set__from_value(0.0).set__to_value(4.00).set__step(0); + desc_max_pos_int.type = 3; // PARAMETER_DOUBLE + desc_max_pos_int.description = std::string("World max integral"); + desc_max_pos_int.floating_point_range = {r_max_pos_int}; + this->declare_parameter("max_pos_int", 0.5f, desc_max_pos_int); + + rcl_interfaces::msg::ParameterDescriptor desc_max_pos_int_b; + rcl_interfaces::msg::FloatingPointRange r_max_pos_int_b; + r_max_pos_int_b.set__from_value(0.0).set__to_value(4.00).set__step(0); + desc_max_pos_int_b.type = 3; // PARAMETER_DOUBLE + desc_max_pos_int_b.description = std::string("Body max integral"); + desc_max_pos_int_b.floating_point_range = {r_max_pos_int_b}; + this->declare_parameter("max_pos_int_b", 0.5f, desc_max_pos_int_b); + + rcl_interfaces::msg::ParameterDescriptor desc_max_tilt_angle; + rcl_interfaces::msg::FloatingPointRange r_max_tilt_angle; + r_max_tilt_angle.set__from_value(0.0).set__to_value(3.14).set__step(0); + desc_max_tilt_angle.type = 3; // PARAMETER_DOUBLE + desc_max_tilt_angle.description = std::string("Max tilt angle"); + desc_max_tilt_angle.floating_point_range = {r_max_tilt_angle}; + this->declare_parameter("max_tilt_angle", static_cast(M_PI), desc_max_tilt_angle); + + // Getting dynamic parameters + this->get_parameter("kp_x", config_kx_[0]); + this->get_parameter("kp_y", config_kx_[1]); + this->get_parameter("kp_z", config_kx_[2]); kx_[0] = config_kx_[0]; kx_[1] = config_kx_[1]; kx_[2] = config_kx_[2]; - config.kp_x = config_kx_[0]; - config.kp_y = config_kx_[1]; - config.kp_z = config_kx_[2]; - n.param("gains/vel/x", config_kv_[0], 4.8f); - n.param("gains/vel/y", config_kv_[1], 4.8f); - n.param("gains/vel/z", config_kv_[2], 6.0f); + this->get_parameter("kd_x", config_kv_[0]); + this->get_parameter("kd_y", config_kv_[1]); + this->get_parameter("kd_z", config_kv_[2]); kv_[0] = config_kv_[0]; kv_[1] = config_kv_[1]; kv_[2] = config_kv_[2]; - config.kd_x = config_kv_[0]; - config.kd_y = config_kv_[1]; - config.kd_z = config_kv_[2]; - - n.param("gains/ki/x", config_ki_[0], 0.0f); - n.param("gains/ki/y", config_ki_[1], 0.0f); - n.param("gains/ki/z", config_ki_[2], 0.0f); - config.ki_x = config_ki_[0]; - config.ki_y = config_ki_[1]; - config.ki_z = config_ki_[2]; - - n.param("gains/ki/yaw", ki_yaw_, 0.0f); - - n.param("gains/kib/x", config_kib_[0], 0.0f); - n.param("gains/kib/y", config_kib_[1], 0.0f); - n.param("gains/kib/z", config_kib_[2], 0.0f); - config.kib_x = config_kib_[0]; - config.kib_y = config_kib_[1]; - config.kib_z = config_kib_[2]; - - n.param("gains/rot/x", kR_[0], 1.5f); - n.param("gains/rot/y", kR_[1], 1.5f); - n.param("gains/rot/z", kR_[2], 1.0f); - n.param("gains/ang/x", kOm_[0], 0.13f); - n.param("gains/ang/y", kOm_[1], 0.13f); - n.param("gains/ang/z", kOm_[2], 0.1f); - config.rot_x = kR_[0]; - config.rot_y = kR_[1]; - config.rot_z = kR_[2]; - config.ang_x = kOm_[0]; - config.ang_y = kOm_[1]; - config.ang_z = kOm_[2]; - - n.param("corrections/kf", corrections_[0], 0.0f); - n.param("corrections/r", corrections_[1], 0.0f); - n.param("corrections/p", corrections_[2], 0.0f); - config.kf_correction = corrections_[0]; - config.roll_correction = corrections_[1]; - config.pitch_correction = corrections_[2]; + + this->get_parameter("ki_x", config_ki_[0]); + this->get_parameter("ki_y", config_ki_[1]); + this->get_parameter("ki_z", config_ki_[2]); + + this->get_parameter("kib_x", config_kib_[0]); + this->get_parameter("kib_y", config_kib_[1]); + this->get_parameter("kib_z", config_kib_[2]); + + this->get_parameter("rot_x", kr_[0]); + this->get_parameter("rot_y", kr_[1]); + this->get_parameter("rot_z", kr_[2]); + + this->get_parameter("ang_x", kom_[0]); + this->get_parameter("ang_y", kom_[1]); + this->get_parameter("ang_z", kom_[2]); + + this->get_parameter("kf_correction", corrections_[0]); + this->get_parameter("roll_correction", corrections_[1]); + this->get_parameter("pitch_correction", corrections_[2]); float max_pos_int, max_pos_int_b; - n.param("max_pos_int", max_pos_int, 0.5f); - n.param("mas_pos_int_b", max_pos_int_b, 0.5f); + this->get_parameter("max_pos_int", max_pos_int); + this->get_parameter("max_pos_int_b", max_pos_int_b); controller_.setMaxIntegral(max_pos_int); controller_.setMaxIntegralBody(max_pos_int_b); - config.max_pos_int = max_pos_int; - config.max_pos_int_b = max_pos_int_b; float max_tilt_angle; - n.param("max_tilt_angle", max_tilt_angle, static_cast(M_PI)); + this->get_parameter("max_tilt_angle", max_tilt_angle); controller_.setMaxTiltAngle(max_tilt_angle); - config.max_tilt_angle = max_tilt_angle; - - // Initialize dynamic reconfigure - reconfigure_server_ = boost::make_shared(config_mutex_, n); - reconfigure_server_->updateConfig(config); - reconfigure_server_->setCallback(boost::bind(&SO3TRPYControlNodelet::cfg_callback, this, _1, _2)); - - trpy_command_pub_ = n.advertise("trpy_cmd", 10); - - odom_sub_ = n.subscribe("odom", 10, &SO3TRPYControlNodelet::odom_callback, this, ros::TransportHints().tcpNoDelay()); - position_cmd_sub_ = n.subscribe("position_cmd", 10, &SO3TRPYControlNodelet::position_cmd_callback, this, - ros::TransportHints().tcpNoDelay()); - enable_motors_sub_ = n.subscribe("motors", 2, &SO3TRPYControlNodelet::enable_motors_callback, this, - ros::TransportHints().tcpNoDelay()); - corrections_sub_ = n.subscribe("corrections", 10, &SO3TRPYControlNodelet::corrections_callback, this, - ros::TransportHints().tcpNoDelay()); + + // Initialize dynamic reconfigure callback + reconfigure_handle_ = this->add_on_set_parameters_callback( + std::bind(&SO3TRPYControlComponent::cfg_callback, this, std::placeholders::_1)); + + trpy_command_pub_ = this->create_publisher("~/trpy_cmd", 10); + + // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos1 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 2), qos_profile); + + odom_sub_ = this->create_subscription( + "~/odom", qos1, std::bind(&SO3TRPYControlComponent::odom_callback, this, std::placeholders::_1)); + position_cmd_sub_ = this->create_subscription( + "~/position_cmd", qos1, std::bind(&SO3TRPYControlComponent::position_cmd_callback, this, std::placeholders::_1)); + enable_motors_sub_ = this->create_subscription( + "~/motors", qos2, std::bind(&SO3TRPYControlComponent::enable_motors_callback, this, std::placeholders::_1)); + corrections_sub_ = this->create_subscription( + "~/corrections", qos1, std::bind(&SO3TRPYControlComponent::corrections_callback, this, std::placeholders::_1)); } -#include -PLUGINLIB_EXPORT_CLASS(SO3TRPYControlNodelet, nodelet::Nodelet); +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(SO3TRPYControlComponent) diff --git a/kr_mav_controllers/test/launch/so3_control_component_test.test.py b/kr_mav_controllers/test/launch/so3_control_component_test.test.py new file mode 100755 index 00000000..54c5ab8c --- /dev/null +++ b/kr_mav_controllers/test/launch/so3_control_component_test.test.py @@ -0,0 +1,91 @@ +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.actions import Node +import launch_testing +import launch_testing.actions +import launch +import unittest +import os + +def generate_test_description(): + + component_under_test = ComposableNodeContainer( + name="so3_container", + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions= [ + ComposableNode( + package="kr_mav_controllers", + plugin="SO3ControlComponent", + name="SO3ControlComponent", + remappings=[ + ("~/odom", "odom"), + ("~/position_cmd", "position_cmd"), + ("~/motors", "motors"), + ("~/corrections", "corrections"), + ("~/so3_cmd", "so3_cmd") + ] + ) + ], + output="screen" + ) + + so3_control_component_test = Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "so3_control_component_test", + ] + ), + output="screen", + ) + + # test_executable_path = os.path.join( + # get_package_share_directory("kr_mav_controllers"), + # 'test', # Executables are usually installed to lib/ in ROS 2 + # "so3_control_component_test.cpp" + # ) + # so3_control_component_test = Node( + # package="kr_mav_controllers", + # executable="so3_control_component_test", + # name="so3_control_tester", + # output="screen" + # ) + + + return LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name="test_binary_dir", + description="Binary directory of package " + "containing test executables", + ), + component_under_test, + so3_control_component_test, + launch_testing.actions.ReadyToTest(), + ]), {'component_under_test': component_under_test, + 'so3_control_component_test': so3_control_component_test} + + # return LaunchDescription([ + # component_under_test, + # so3_control_component_test, + # launch_testing.actions.ReadyToTest() + # ]), {'component_under_test': component_under_test, + # 'so3_control_component_test': so3_control_component_test} + +class TestGTestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_gtest_run_complete(self, proc_info, so3_control_component_test): + proc_info.assertWaitForShutdown(so3_control_component_test, timeout=1000.0) + + +@launch_testing.post_shutdown_test() +class TestGTestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes + def test_gtest_pass(self, proc_info, so3_control_component_test): + launch_testing.asserts.assertExitCodes( + proc_info, process=so3_control_component_test + ) + diff --git a/kr_mav_controllers/test/so3_control_component_test.cpp b/kr_mav_controllers/test/so3_control_component_test.cpp new file mode 100644 index 00000000..13450ee1 --- /dev/null +++ b/kr_mav_controllers/test/so3_control_component_test.cpp @@ -0,0 +1,241 @@ +#include + +#include +#include + +#include "kr_mav_controllers/so3_control_tester.hpp" + +class SO3ControlComponentTest : public testing::Test +{ + public: + SO3ControlComponentTest() + : tester(std::make_shared()), + executor(std::make_shared()) + { + } + + std::shared_ptr tester; + std::thread spin_thread; + std::shared_ptr executor; + + // Spinning the node in a different thread + void SetUp() override + { + executor->add_node(tester); + spin_thread = std::thread([this]() { executor->spin(); }); + rclcpp::sleep_for(1s); + } + + // Cancel spinning of node + void TearDown() override + { + executor->cancel(); + spin_thread.join(); + } +}; + +/* + * @brief Test1: test if SO3Command is received without enabling any motors or odom data or position command + * Expected behavior: so3_command_received_ = false + * Also checking if the component is active + */ +TEST_F(SO3ControlComponentTest, Test1) +{ + ASSERT_TRUE(tester->is_so3_cmd_publisher_active()); // checking if nodelet is active + std::cout << "Performing Test1!\n"; + { + std::lock_guard lock(tester->mutex); + EXPECT_FALSE(tester->so3_command_received_); + } +} + +/* + * @brief Test2: publish position command and check so3 command + * Expected behavior: so3_command_received_ = false + */ +TEST_F(SO3ControlComponentTest, Test2) +{ + std::cout << "Performing Test2!\n"; + tester->publish_single_position_command(); + rclcpp::sleep_for(1s); + { + std::lock_guard lock(tester->mutex); + EXPECT_FALSE(tester->so3_command_received_); + } + tester->reset_so3_cmd_pointer(); +} + +/* + * @brief Test3: enable motors and publish position command. check so3 command + * Expected behavior: so3_command_received_ = false + */ +TEST_F(SO3ControlComponentTest, Test3) +{ + tester->publish_enable_motors(true); + tester->publish_single_position_command(); + rclcpp::sleep_for(1s); + { + std::lock_guard lock(tester->mutex); + EXPECT_FALSE(tester->so3_command_received_); + } + tester->reset_so3_cmd_pointer(); +} + +/* + * @brief Test4: enable motors, publish odom, publish position command. check so3 command + * Expected behavior: so3_command_received_ = true + */ +TEST_F(SO3ControlComponentTest, Test4) +{ + tester->publish_enable_motors(true); + tester->populate_odom_msgs(); + tester->publish_odom_msg(0); + rclcpp::sleep_for(1s); + tester->publish_single_position_command(); + rclcpp::sleep_for(1s); + Test4Data ref; + { + std::lock_guard lock(tester->mutex); + EXPECT_TRUE(tester->so3_command_received_); + EXPECT_EQ(tester->so3_cmd_->force.x, ref.force_x); + EXPECT_EQ(tester->so3_cmd_->force.y, ref.force_y); + EXPECT_EQ(tester->so3_cmd_->force.z, ref.force_z); + EXPECT_EQ(tester->so3_cmd_->orientation.x, ref.orientation_x); + EXPECT_EQ(tester->so3_cmd_->orientation.y, ref.orientation_y); + EXPECT_EQ(tester->so3_cmd_->orientation.z, ref.orientation_z); + EXPECT_EQ(tester->so3_cmd_->orientation.w, ref.orientation_w); + EXPECT_EQ(tester->so3_cmd_->angular_velocity.x, ref.angular_velocity_x); + EXPECT_EQ(tester->so3_cmd_->angular_velocity.y, ref.angular_velocity_y); + EXPECT_EQ(tester->so3_cmd_->angular_velocity.z, ref.angular_velocity_z); + EXPECT_EQ(tester->so3_cmd_->aux.current_yaw, ref.current_yaw); + EXPECT_EQ(tester->so3_cmd_->aux.kf_correction, ref.kf_correction); + EXPECT_EQ(tester->so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0]); + EXPECT_EQ(tester->so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1]); + } + tester->reset_so3_cmd_pointer(); +} + +/* + * @brief Test5: enable motors, publish odom, publish position commands (11 cmds). Check so3 command + */ +TEST_F(SO3ControlComponentTest, Test5) +{ + tester->publish_enable_motors(true); + tester->populate_odom_msgs(); + tester->populate_position_cmd_vector(1, 1, 2, 0, 0.1); + tester->publish_odom_msg(0); + rclcpp::sleep_for(1s); + Test5Data ref; + for(int i = 0; i < 11; i++) + { + tester->publish_position_command(i); + rclcpp::sleep_for(1s); + { + std::lock_guard lock(tester->mutex); + EXPECT_TRUE(tester->so3_command_received_); + EXPECT_NEAR(tester->so3_cmd_->force.x, ref.force_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->force.y, ref.force_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->force.z, ref.force_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.x, ref.orientation_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.y, ref.orientation_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.z, ref.orientation_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.w, ref.orientation_w[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.x, ref.angular_velocity_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.y, ref.angular_velocity_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.z, ref.angular_velocity_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.current_yaw, ref.current_yaw[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.kf_correction, ref.kf_correction, 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1], 1e-4); + } + tester->reset_so3_cmd_pointer(); + } +} + +/* + * @brief Test6: enable motors, publish odom, publish position commands (11 cmds). Check so3 command + */ +TEST_F(SO3ControlComponentTest, Test6) +{ + tester->publish_enable_motors(true); + tester->populate_odom_msgs(); + tester->populate_position_cmd_vector(2, 3, 5, 7, 0.2); + tester->publish_odom_msg(1); + rclcpp::sleep_for(1s); + Test6Data ref; + for(int i = 0; i < 11; i++) + { + tester->publish_position_command(i); + rclcpp::sleep_for(1s); + { + std::lock_guard lock(tester->mutex); + EXPECT_TRUE(tester->so3_command_received_); + EXPECT_NEAR(tester->so3_cmd_->force.x, ref.force_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->force.y, ref.force_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->force.z, ref.force_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.x, ref.orientation_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.y, ref.orientation_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.z, ref.orientation_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.w, ref.orientation_w[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.x, ref.angular_velocity_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.y, ref.angular_velocity_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.z, ref.angular_velocity_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.current_yaw, ref.current_yaw[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.kf_correction, ref.kf_correction, 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1], 1e-4); + } + tester->reset_so3_cmd_pointer(); + } +} + +/* + * @brief Test7: enable motors, publish corrections, publish odom, publish position commands (11 cmds). Check so3 + * command + */ +TEST_F(SO3ControlComponentTest, Test7) +{ + tester->publish_enable_motors(true); + tester->populate_odom_msgs(); + tester->populate_position_cmd_vector(4, 2, 1, 56, 0.5); + float kf_corr = 1.0f; + float ang_corr[2] = {0.2f, 0.3f}; + tester->publish_corrections(kf_corr, ang_corr); + rclcpp::sleep_for(1s); + tester->publish_odom_msg(2); + rclcpp::sleep_for(1s); + Test7Data ref; + for(int i = 0; i < 11; i++) + { + tester->publish_position_command(i); + rclcpp::sleep_for(1s); + { + std::lock_guard lock(tester->mutex); + EXPECT_TRUE(tester->so3_command_received_); + EXPECT_NEAR(tester->so3_cmd_->force.x, ref.force_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->force.y, ref.force_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->force.z, ref.force_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.x, ref.orientation_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.y, ref.orientation_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.z, ref.orientation_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.w, ref.orientation_w[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.x, ref.angular_velocity_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.y, ref.angular_velocity_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.z, ref.angular_velocity_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.current_yaw, ref.current_yaw[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.kf_correction, ref.kf_correction, 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1], 1e-4); + } + tester->reset_so3_cmd_pointer(); + } +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + auto result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/kr_mav_controllers/test/so3_control_nodelet.test b/kr_mav_controllers/test/so3_control_nodelet.test deleted file mode 100644 index 3977294e..00000000 --- a/kr_mav_controllers/test/so3_control_nodelet.test +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/kr_mav_controllers/test/so3_control_nodelet_test.cpp b/kr_mav_controllers/test/so3_control_nodelet_test.cpp deleted file mode 100644 index 1df5f62d..00000000 --- a/kr_mav_controllers/test/so3_control_nodelet_test.cpp +++ /dev/null @@ -1,230 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include "kr_mav_controllers/so3_control_tester.hpp" - -/* - * @brief Test1: test if SO3Command is received without enabling any motors or odom data or position command - * Expected behavior: so3_command_received_ = false - * Also checking if the nodelet is active - */ -TEST(SO3ControlNodeletTest, Test1) -{ - std::cout << "Performing Test1!\n"; - SO3ControlTester tester; - { - std::lock_guard lock(tester.mutex); - EXPECT_FALSE(tester.so3_command_received_); - } - ASSERT_TRUE(tester.is_so3_cmd_publisher_active()); // checking if nodelet is active -} - -/* - * @brief Test2: publish position command and check so3 command - * Expected behavior: so3_command_received_ = false - */ -TEST(SO3ControlNodeletTest, Test2) -{ - SO3ControlTester tester; - tester.publish_single_position_command(); - ros::Duration(1.0).sleep(); - { - std::lock_guard lock(tester.mutex); - EXPECT_FALSE(tester.so3_command_received_); - } - tester.reset_so3_cmd_pointer(); -} - -/* - * @brief Test3: enable motors and publish position command. check so3 command - * Expected behavior: so3_command_received_ = false - */ -TEST(SO3ControlNodeletTest, Test3) -{ - SO3ControlTester tester; - tester.publish_enable_motors(true); - tester.publish_single_position_command(); - ros::Duration(1.0).sleep(); - { - std::lock_guard lock(tester.mutex); - EXPECT_FALSE(tester.so3_command_received_); - } - tester.reset_so3_cmd_pointer(); -} - -/* - * @brief Test4: enable motors, publish odom, publish position command. check so3 command - * Expected behavior: so3_command_received_ = true - */ -TEST(SO3ControlNodeletTest, Test4) -{ - SO3ControlTester tester; - tester.publish_enable_motors(true); - tester.populate_odom_msgs(); - tester.publish_odom_msg(0); - ros::Duration(1.0).sleep(); - tester.publish_single_position_command(); - ros::Duration(1.0).sleep(); - Test4Data ref; - { - std::lock_guard lock(tester.mutex); - EXPECT_TRUE(tester.so3_command_received_); - EXPECT_EQ(tester.so3_cmd_->force.x, ref.force_x); - EXPECT_EQ(tester.so3_cmd_->force.y, ref.force_y); - EXPECT_EQ(tester.so3_cmd_->force.z, ref.force_z); - EXPECT_EQ(tester.so3_cmd_->orientation.x, ref.orientation_x); - EXPECT_EQ(tester.so3_cmd_->orientation.y, ref.orientation_y); - EXPECT_EQ(tester.so3_cmd_->orientation.z, ref.orientation_z); - EXPECT_EQ(tester.so3_cmd_->orientation.w, ref.orientation_w); - EXPECT_EQ(tester.so3_cmd_->angular_velocity.x, ref.angular_velocity_x); - EXPECT_EQ(tester.so3_cmd_->angular_velocity.y, ref.angular_velocity_y); - EXPECT_EQ(tester.so3_cmd_->angular_velocity.z, ref.angular_velocity_z); - EXPECT_EQ(tester.so3_cmd_->aux.current_yaw, ref.current_yaw); - EXPECT_EQ(tester.so3_cmd_->aux.kf_correction, ref.kf_correction); - EXPECT_EQ(tester.so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0]); - EXPECT_EQ(tester.so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1]); - } - tester.reset_so3_cmd_pointer(); -} - -/* -* @brief Test5: enable motors, publish odom, publish position commands (11 cmds). Check so3 command -*/ -TEST(SO3ControlNodeletTest, Test5) -{ - SO3ControlTester tester; - tester.publish_enable_motors(true); - tester.populate_odom_msgs(); - tester.populate_position_cmd_vector(1, 1, 2, 0, 0.1); - tester.publish_odom_msg(0); - ros::Duration(1.0).sleep(); - Test5Data ref; - for(int i = 0; i < 11; i++) - { - tester.publish_position_command(i); - ros::Duration(1.0).sleep(); - { - std::lock_guard lock(tester.mutex); - EXPECT_TRUE(tester.so3_command_received_); - EXPECT_NEAR(tester.so3_cmd_->force.x, ref.force_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->force.y, ref.force_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->force.z, ref.force_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.x, ref.orientation_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.y, ref.orientation_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.z, ref.orientation_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.w, ref.orientation_w[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.x, ref.angular_velocity_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.y, ref.angular_velocity_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.z, ref.angular_velocity_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.current_yaw, ref.current_yaw[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.kf_correction, ref.kf_correction, 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1], 1e-4); - } - tester.reset_so3_cmd_pointer(); - } -} - -/* -* @brief Test6: enable motors, publish odom, publish position commands (11 cmds). Check so3 command -*/ -TEST(SO3ControlNodeletTest, Test6) -{ - SO3ControlTester tester; - tester.publish_enable_motors(true); - tester.populate_odom_msgs(); - tester.populate_position_cmd_vector(2, 3, 5, 7, 0.2); - tester.publish_odom_msg(1); - ros::Duration(1.0).sleep(); - Test6Data ref; - for(int i = 0; i < 11; i++) - { - tester.publish_position_command(i); - ros::Duration(1.0).sleep(); - { - std::lock_guard lock(tester.mutex); - EXPECT_TRUE(tester.so3_command_received_); - EXPECT_NEAR(tester.so3_cmd_->force.x, ref.force_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->force.y, ref.force_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->force.z, ref.force_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.x, ref.orientation_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.y, ref.orientation_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.z, ref.orientation_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.w, ref.orientation_w[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.x, ref.angular_velocity_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.y, ref.angular_velocity_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.z, ref.angular_velocity_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.current_yaw, ref.current_yaw[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.kf_correction, ref.kf_correction, 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1], 1e-4); - } - tester.reset_so3_cmd_pointer(); - } -} - -/* -* @brief Test7: enable motors, publish corrections, publish odom, publish position commands (11 cmds). Check so3 command -*/ -TEST(SO3ControlNodeletTest, Test7) -{ - SO3ControlTester tester; - tester.publish_enable_motors(true); - tester.populate_odom_msgs(); - tester.populate_position_cmd_vector(4, 2, 1, 56, 0.5); - float kf_corr = 1.0f; - float ang_corr[2] = {0.2f, 0.3f}; - tester.publish_corrections(kf_corr, ang_corr); - ros::Duration(1.0).sleep(); - tester.publish_odom_msg(2); - ros::Duration(1.0).sleep(); - Test7Data ref; - for(int i = 0; i < 11; i++) - { - tester.publish_position_command(i); - ros::Duration(1.0).sleep(); - { - std::lock_guard lock(tester.mutex); - EXPECT_TRUE(tester.so3_command_received_); - EXPECT_NEAR(tester.so3_cmd_->force.x, ref.force_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->force.y, ref.force_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->force.z, ref.force_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.x, ref.orientation_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.y, ref.orientation_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.z, ref.orientation_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.w, ref.orientation_w[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.x, ref.angular_velocity_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.y, ref.angular_velocity_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.z, ref.angular_velocity_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.current_yaw, ref.current_yaw[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.kf_correction, ref.kf_correction, 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1], 1e-4); - } - tester.reset_so3_cmd_pointer(); - } -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "so3_control_nodelet_tester"); - testing::InitGoogleTest(&argc, argv); - - std::thread t([] { - while (ros::ok()) - ros::spin(); - }); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - t.join(); - - return res; -} From 64981e40c72cbdb65c7d6acc980c85170dcc46f3 Mon Sep 17 00:00:00 2001 From: Fernando Cladera Date: Wed, 13 Mar 2024 18:35:00 -0400 Subject: [PATCH 08/64] Disable failing cpplint tests that conflict with coding style --- .github/workflows/cpplint.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/cpplint.yml b/.github/workflows/cpplint.yml index 0bea07fd..f6d19e5d 100644 --- a/.github/workflows/cpplint.yml +++ b/.github/workflows/cpplint.yml @@ -15,6 +15,8 @@ jobs: ,-whitespace/braces\ ,-whitespace/parens\ ,-whitespace/newline\ + ,-build/header_guard\ + ,-readability/todo\ ,-build/c++11" # -readability/braces\ # ,-whitespace/comments\ From cdde203d5a8943614f6e348fa2f286ceab6482ac Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Wed, 13 Mar 2024 19:04:09 -0400 Subject: [PATCH 09/64] cpplint compliant changes --- .../kr_mav_controllers/so3_control_tester.hpp | 7 +++--- .../src/pid_control_component.cpp | 2 +- .../src/so3_control_component.cpp | 2 +- kr_mav_controllers/src/so3_trpy_control.cpp | 2 +- .../test/so3_control_component_test.cpp | 24 +++++++++---------- 5 files changed, 19 insertions(+), 18 deletions(-) diff --git a/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp b/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp index 623d5071..b5358654 100644 --- a/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp +++ b/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp @@ -1,3 +1,6 @@ +#include +#include + #include "kr_mav_msgs/msg/corrections.hpp" #include "kr_mav_msgs/msg/position_command.hpp" #include "kr_mav_msgs/msg/so3_command.hpp" @@ -5,8 +8,6 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/bool.hpp" -using namespace std::chrono_literals; - class SO3ControlTester : public rclcpp::Node { public: @@ -58,7 +59,7 @@ bool SO3ControlTester::is_so3_cmd_publisher_active() flag = true; else { - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); if(so3_cmd_sub_->get_publisher_count() > 0) flag = true; else diff --git a/kr_mav_controllers/src/pid_control_component.cpp b/kr_mav_controllers/src/pid_control_component.cpp index 35381fa3..bb6a0351 100644 --- a/kr_mav_controllers/src/pid_control_component.cpp +++ b/kr_mav_controllers/src/pid_control_component.cpp @@ -14,7 +14,7 @@ class PIDControlComponent : public rclcpp::Node { public: - PIDControlComponent(const rclcpp::NodeOptions &options); + explicit PIDControlComponent(const rclcpp::NodeOptions &options); private: void publishTRPYCommand(void); diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index 823a8946..9a80d54b 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -14,7 +14,7 @@ class SO3ControlComponent : public rclcpp::Node { public: - SO3ControlComponent(const rclcpp::NodeOptions &options); + explicit SO3ControlComponent(const rclcpp::NodeOptions &options); private: void publishSO3Command(); diff --git a/kr_mav_controllers/src/so3_trpy_control.cpp b/kr_mav_controllers/src/so3_trpy_control.cpp index 3c4b750f..d212ee63 100644 --- a/kr_mav_controllers/src/so3_trpy_control.cpp +++ b/kr_mav_controllers/src/so3_trpy_control.cpp @@ -13,7 +13,7 @@ class SO3TRPYControlComponent : public rclcpp::Node { public: - SO3TRPYControlComponent(const rclcpp::NodeOptions &options); + explicit SO3TRPYControlComponent(const rclcpp::NodeOptions &options); private: void publishCommand(); diff --git a/kr_mav_controllers/test/so3_control_component_test.cpp b/kr_mav_controllers/test/so3_control_component_test.cpp index 13450ee1..a7928597 100644 --- a/kr_mav_controllers/test/so3_control_component_test.cpp +++ b/kr_mav_controllers/test/so3_control_component_test.cpp @@ -23,7 +23,7 @@ class SO3ControlComponentTest : public testing::Test { executor->add_node(tester); spin_thread = std::thread([this]() { executor->spin(); }); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); } // Cancel spinning of node @@ -57,7 +57,7 @@ TEST_F(SO3ControlComponentTest, Test2) { std::cout << "Performing Test2!\n"; tester->publish_single_position_command(); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); { std::lock_guard lock(tester->mutex); EXPECT_FALSE(tester->so3_command_received_); @@ -73,7 +73,7 @@ TEST_F(SO3ControlComponentTest, Test3) { tester->publish_enable_motors(true); tester->publish_single_position_command(); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); { std::lock_guard lock(tester->mutex); EXPECT_FALSE(tester->so3_command_received_); @@ -90,9 +90,9 @@ TEST_F(SO3ControlComponentTest, Test4) tester->publish_enable_motors(true); tester->populate_odom_msgs(); tester->publish_odom_msg(0); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); tester->publish_single_position_command(); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); Test4Data ref; { std::lock_guard lock(tester->mutex); @@ -124,12 +124,12 @@ TEST_F(SO3ControlComponentTest, Test5) tester->populate_odom_msgs(); tester->populate_position_cmd_vector(1, 1, 2, 0, 0.1); tester->publish_odom_msg(0); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); Test5Data ref; for(int i = 0; i < 11; i++) { tester->publish_position_command(i); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); { std::lock_guard lock(tester->mutex); EXPECT_TRUE(tester->so3_command_received_); @@ -161,12 +161,12 @@ TEST_F(SO3ControlComponentTest, Test6) tester->populate_odom_msgs(); tester->populate_position_cmd_vector(2, 3, 5, 7, 0.2); tester->publish_odom_msg(1); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); Test6Data ref; for(int i = 0; i < 11; i++) { tester->publish_position_command(i); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); { std::lock_guard lock(tester->mutex); EXPECT_TRUE(tester->so3_command_received_); @@ -201,14 +201,14 @@ TEST_F(SO3ControlComponentTest, Test7) float kf_corr = 1.0f; float ang_corr[2] = {0.2f, 0.3f}; tester->publish_corrections(kf_corr, ang_corr); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); tester->publish_odom_msg(2); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); Test7Data ref; for(int i = 0; i < 11; i++) { tester->publish_position_command(i); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); { std::lock_guard lock(tester->mutex); EXPECT_TRUE(tester->so3_command_received_); From 2aa814eb1a8fb8852d184935295a0d3e6d3f4943 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Wed, 13 Mar 2024 21:21:07 -0400 Subject: [PATCH 10/64] refined comments --- .../launch/so3_control_component_test.test.py | 25 ++----------------- 1 file changed, 2 insertions(+), 23 deletions(-) diff --git a/kr_mav_controllers/test/launch/so3_control_component_test.test.py b/kr_mav_controllers/test/launch/so3_control_component_test.test.py index 54c5ab8c..d3382f0a 100755 --- a/kr_mav_controllers/test/launch/so3_control_component_test.test.py +++ b/kr_mav_controllers/test/launch/so3_control_component_test.test.py @@ -7,10 +7,10 @@ import launch_testing.actions import launch import unittest -import os def generate_test_description(): + # initialize component component_under_test = ComposableNodeContainer( name="so3_container", namespace="", @@ -33,6 +33,7 @@ def generate_test_description(): output="screen" ) + # initialize test node so3_control_component_test = Node( executable=launch.substitutions.PathJoinSubstitution( [ @@ -43,19 +44,6 @@ def generate_test_description(): output="screen", ) - # test_executable_path = os.path.join( - # get_package_share_directory("kr_mav_controllers"), - # 'test', # Executables are usually installed to lib/ in ROS 2 - # "so3_control_component_test.cpp" - # ) - # so3_control_component_test = Node( - # package="kr_mav_controllers", - # executable="so3_control_component_test", - # name="so3_control_tester", - # output="screen" - # ) - - return LaunchDescription([ launch.actions.DeclareLaunchArgument( name="test_binary_dir", @@ -68,19 +56,11 @@ def generate_test_description(): ]), {'component_under_test': component_under_test, 'so3_control_component_test': so3_control_component_test} - # return LaunchDescription([ - # component_under_test, - # so3_control_component_test, - # launch_testing.actions.ReadyToTest() - # ]), {'component_under_test': component_under_test, - # 'so3_control_component_test': so3_control_component_test} - class TestGTestWaitForCompletion(unittest.TestCase): # Waits for test to complete, then waits a bit to make sure result files are generated def test_gtest_run_complete(self, proc_info, so3_control_component_test): proc_info.assertWaitForShutdown(so3_control_component_test, timeout=1000.0) - @launch_testing.post_shutdown_test() class TestGTestProcessPostShutdown(unittest.TestCase): # Checks if the test has been completed with acceptable exit codes @@ -88,4 +68,3 @@ def test_gtest_pass(self, proc_info, so3_control_component_test): launch_testing.asserts.assertExitCodes( proc_info, process=so3_control_component_test ) - From e5fbc6ca583c2bd494018424591f1ee46927d292 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Fri, 15 Mar 2024 15:40:32 -0400 Subject: [PATCH 11/64] removed colcon test block --- kr_mesh_visualization/CMakeLists.txt | 12 ------------ trackers/kr_tracker_msgs/CMakeLists.txt | 11 ----------- 2 files changed, 23 deletions(-) diff --git a/kr_mesh_visualization/CMakeLists.txt b/kr_mesh_visualization/CMakeLists.txt index 0bb9f529..7120fb56 100644 --- a/kr_mesh_visualization/CMakeLists.txt +++ b/kr_mesh_visualization/CMakeLists.txt @@ -26,16 +26,4 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/trackers/kr_tracker_msgs/CMakeLists.txt b/trackers/kr_tracker_msgs/CMakeLists.txt index d8eb6bdf..3e3f9158 100644 --- a/trackers/kr_tracker_msgs/CMakeLists.txt +++ b/trackers/kr_tracker_msgs/CMakeLists.txt @@ -38,17 +38,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} DEPENDENCIES std_msgs builtin_interfaces geometry_msgs ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - ament_export_dependencies(rosidl_default_runtime) ament_package() From a053d90365ff1c7a96503072c214887ee2e332f8 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Mon, 25 Mar 2024 15:57:27 -0400 Subject: [PATCH 12/64] rqt gui and crazyflie interface ros2 compatible --- .../kr_crazyflie_interface/CMakeLists.txt | 63 ++-- .../config/crazyflie.yaml | 16 +- .../kr_crazyflie_interface/launch/test.launch | 18 -- .../launch/test.launch.py | 54 ++++ .../kr_crazyflie_interface/nodelet_plugin.xml | 7 - interfaces/kr_crazyflie_interface/package.xml | 15 +- .../src/so3cmd_to_crazyflie_component.cpp | 232 +++++++++++++++ .../src/so3cmd_to_crazyflie_nodelet.cpp | 231 --------------- rqt_mav_manager/CMakeLists.txt | 13 - rqt_mav_manager/package.xml | 34 ++- rqt_mav_manager/resource/rqt_mav_manager | 0 rqt_mav_manager/rqt_mav_manager/__init__.py | 273 ++++++++++++++++++ .../rqt_mav_manager/rqt_mav_manager.py | 13 + rqt_mav_manager/scripts/rqt_mav_manager | 8 - rqt_mav_manager/setup.cfg | 4 + rqt_mav_manager/setup.py | 33 ++- .../src/rqt_mav_manager/__init__.py | 211 -------------- 17 files changed, 663 insertions(+), 562 deletions(-) delete mode 100644 interfaces/kr_crazyflie_interface/launch/test.launch create mode 100644 interfaces/kr_crazyflie_interface/launch/test.launch.py delete mode 100644 interfaces/kr_crazyflie_interface/nodelet_plugin.xml create mode 100644 interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp delete mode 100644 interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp delete mode 100644 rqt_mav_manager/CMakeLists.txt create mode 100644 rqt_mav_manager/resource/rqt_mav_manager create mode 100644 rqt_mav_manager/rqt_mav_manager/__init__.py create mode 100644 rqt_mav_manager/rqt_mav_manager/rqt_mav_manager.py delete mode 100755 rqt_mav_manager/scripts/rqt_mav_manager create mode 100644 rqt_mav_manager/setup.cfg delete mode 100644 rqt_mav_manager/src/rqt_mav_manager/__init__.py diff --git a/interfaces/kr_crazyflie_interface/CMakeLists.txt b/interfaces/kr_crazyflie_interface/CMakeLists.txt index 9029cf6f..594c419f 100644 --- a/interfaces/kr_crazyflie_interface/CMakeLists.txt +++ b/interfaces/kr_crazyflie_interface/CMakeLists.txt @@ -1,48 +1,35 @@ cmake_minimum_required(VERSION 3.10) project(kr_crazyflie_interface) -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) - -find_package( - catkin REQUIRED - COMPONENTS roscpp - nav_msgs - geometry_msgs - kr_mav_msgs - nodelet) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(kr_mav_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(Eigen3 REQUIRED) -include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) - -catkin_package( - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - roscpp - nav_msgs - geometry_msgs - kr_mav_msgs - nodelet - DEPENDS - EIGEN3) +add_library(${PROJECT_NAME} SHARED src/so3cmd_to_crazyflie_component.cpp) +ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_components kr_mav_msgs nav_msgs geometry_msgs) +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) +rclcpp_components_register_nodes(${PROJECT_NAME} "SO3CmdToCrazyflie") -add_library(${PROJECT_NAME} src/so3cmd_to_crazyflie_nodelet.cpp) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) -install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) -install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/interfaces/kr_crazyflie_interface/config/crazyflie.yaml b/interfaces/kr_crazyflie_interface/config/crazyflie.yaml index 80f41c9d..b2e0bf59 100644 --- a/interfaces/kr_crazyflie_interface/config/crazyflie.yaml +++ b/interfaces/kr_crazyflie_interface/config/crazyflie.yaml @@ -1,7 +1,9 @@ -kp_yaw_rate: 0.1 -c1: -0.6709 -c2: 0.1932 -c3: 13.0652 -so3_cmd_timeout: 0.1 -thrust_pwm_max: 60000 -thrust_pwm_min: 20000 \ No newline at end of file +so3cmd_to_crazyflie: + ros__parameters: + kp_yaw_rate: 0.1 + c1: -0.6709 + c2: 0.1932 + c3: 13.0652 + so3_cmd_timeout: 0.1 + thrust_pwm_max: 60000 + thrust_pwm_min: 20000 diff --git a/interfaces/kr_crazyflie_interface/launch/test.launch b/interfaces/kr_crazyflie_interface/launch/test.launch deleted file mode 100644 index 582852a0..00000000 --- a/interfaces/kr_crazyflie_interface/launch/test.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - diff --git a/interfaces/kr_crazyflie_interface/launch/test.launch.py b/interfaces/kr_crazyflie_interface/launch/test.launch.py new file mode 100644 index 00000000..fa802026 --- /dev/null +++ b/interfaces/kr_crazyflie_interface/launch/test.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + + robot_ns = LaunchConfiguration('robot') + odom_topic = LaunchConfiguration('odom') + so3_cmd_topic = LaunchConfiguration('so3_cmd') + + robot_arg = DeclareLaunchArgument( + 'robot', default_value='' + ) + odom_arg = DeclareLaunchArgument( + 'odom', default_value='odom' + ) + so3_cmd_arg = DeclareLaunchArgument( + 'so3_cmd', default_value='so3_cmd' + ) + + # Path to the configuration file + config_file = FindPackageShare('kr_crazyflie_interface').find('kr_crazyflie_interface') + '/config/crazyflie.yaml' + + # Component configuration + so3cmd_to_crazyflie_component = ComposableNodeContainer( + name="so3_container", + namespace=LaunchConfiguration('robot'), + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="kr_crazyflie_interface", + plugin="SO3CmdToCrazyflie", + name="so3cmd_to_crazyflie", + parameters=[config_file], + remappings=[ + ("~/odom", odom_topic), + ("~/so3_cmd", so3_cmd_topic) + ] + ) + ], + output='screen' + ) + + return LaunchDescription([ + robot_arg, + odom_arg, + so3_cmd_arg, + so3cmd_to_crazyflie_component + ]) diff --git a/interfaces/kr_crazyflie_interface/nodelet_plugin.xml b/interfaces/kr_crazyflie_interface/nodelet_plugin.xml deleted file mode 100644 index 6337b86d..00000000 --- a/interfaces/kr_crazyflie_interface/nodelet_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - This reformats an so3_command and publishes it on compatabile Crazyflie topics - - - diff --git a/interfaces/kr_crazyflie_interface/package.xml b/interfaces/kr_crazyflie_interface/package.xml index 413943ce..1f0faf39 100644 --- a/interfaces/kr_crazyflie_interface/package.xml +++ b/interfaces/kr_crazyflie_interface/package.xml @@ -1,24 +1,25 @@ - + + + kr_crazyflie_interface 1.0.0 kr_crazyflie_interface Justin Thomas Aaron Weinstein + Pranav Shah BSD - - catkin + ament_cmake - - roscpp + rclcpp nav_msgs geometry_msgs kr_mav_msgs - nodelet + rclcpp_components - + ament_cmake diff --git a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp new file mode 100644 index 00000000..2e585961 --- /dev/null +++ b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp @@ -0,0 +1,232 @@ +// This nodelet is meant to subscribe to so3 commands and then convert them +// to geometry_msgs/Twist which is the type for cmd_vel, the crazyflie control +// topic. The format of this is as follows: +// linear.y = roll [-30 to 30 degrees] (may be negative) +// linear.x = pitch [-30 to 30 degrees] (may be negative) +// linear.z = thrust [0 to 60,000] (motors stiction around 2000) +// angular.z = yawrate [-200 to 200 degrees/second] (note this is not yaw!) + +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "kr_mav_msgs/msg/so3_command.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" + +// TODO: Remove CLAMP as macro +#define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x) + +class SO3CmdToCrazyflie : public rclcpp::Node +{ + public: + explicit SO3CmdToCrazyflie(const rclcpp::NodeOptions &options); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + void so3_cmd_callback(const kr_mav_msgs::msg::SO3Command::UniquePtr msg); + void odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom); + + bool odom_set_, so3_cmd_set_; + Eigen::Quaterniond q_odom_; + + rclcpp::Publisher::SharedPtr crazy_fast_cmd_vel_pub_, crazy_cmd_vel_pub_; + + rclcpp::Subscription::SharedPtr so3_cmd_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + + double so3_cmd_timeout_; + rclcpp::Time last_so3_cmd_time_; + kr_mav_msgs::msg::SO3Command last_so3_cmd_; + + double c1_; + double c2_; + double c3_; + + // TODO get rid of this for the gains coming in + double kp_yaw_rate_; + + int thrust_pwm_min_; // Necessary to overcome stiction + int thrust_pwm_max_; // Mapped to PWM + + int motor_status_; +}; + +void SO3CmdToCrazyflie::odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom) +{ + if(!odom_set_) + odom_set_ = true; + + q_odom_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, + odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); + + if(so3_cmd_set_ && ((this->now() - last_so3_cmd_time_).seconds() >= so3_cmd_timeout_)) + { + auto last_so3_cmd_ptr = std::make_unique(last_so3_cmd_); + + so3_cmd_callback(std::move(last_so3_cmd_ptr)); + } +} + +void SO3CmdToCrazyflie::so3_cmd_callback(kr_mav_msgs::msg::SO3Command::UniquePtr msg) +{ + if(!so3_cmd_set_) + so3_cmd_set_ = true; + + // switch on motors + if(msg->aux.enable_motors) + { + // If the crazyflie motors are timed out, we need to send a zero message in order to get them to start + if(motor_status_ < 3) + { + auto motors_vel_cmd = std::make_unique(); + crazy_cmd_vel_pub_->publish(std::move(motors_vel_cmd)); + last_so3_cmd_ = *msg; + last_so3_cmd_time_ = msg->header.stamp; + motor_status_ += 1; + return; + } + // After sending zero message send min thrust + if(motor_status_ < 10) + { + auto motors_vel_cmd = std::make_unique(); + motors_vel_cmd->linear.z = thrust_pwm_min_; + crazy_cmd_vel_pub_->publish(std::move(motors_vel_cmd)); + } + motor_status_ += 1; + } + else if(!msg->aux.enable_motors) + { + motor_status_ = 0; + auto motors_vel_cmd = std::make_unique(); + crazy_cmd_vel_pub_->publish(std::move(motors_vel_cmd)); + last_so3_cmd_ = *msg; + last_so3_cmd_time_ = msg->header.stamp; + return; + } + + // 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); + + // check psi for stability + const Eigen::Matrix3d R_des(q_des); + const Eigen::Matrix3d R_cur(q_odom_); + + const float yaw_cur = std::atan2(R_cur(1, 0), R_cur(0, 0)); + const float yaw_des = std::atan2(R_des(1, 0), R_des(0, 0)); + + // Map these into the current body frame (based on yaw) + Eigen::Matrix3d R_des_new = R_des * Eigen::AngleAxisd(yaw_cur - yaw_des, Eigen::Vector3d::UnitZ()); + float pitch_des = -std::asin(R_des_new(2, 0)); + float roll_des = std::atan2(R_des_new(2, 1), R_des_new(2, 2)); + + roll_des = roll_des * 180 / M_PI; + pitch_des = pitch_des * 180 / M_PI; + + double thrust_f = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); // Force in Newtons + // RCLCPP_INFO(this->get_logger(), "thrust_f is %2.5f newtons", thrust_f); + thrust_f = std::max(thrust_f, 0.0); + + thrust_f = thrust_f * 1000 / 9.81; // Force in grams + // RCLCPP_INFO(this->get_logger(), "thrust_f is %2.5f grams", thrust_f); + + // RCLCPP_INFO(this->get_logger(), "coeffs are %2.2f, %2.2f, %2.2f", c1_, c2_, c3_); + float thrust_pwm = c1_ + c2_ * std::sqrt(c3_ + thrust_f); + + // RCLCPP_INFO(this->get_logger(), "thrust_pwm is %2.5f from 0-1", thrust_pwm); + + thrust_pwm = thrust_pwm * thrust_pwm_max_; // thrust_pwm mapped from 0-60000 + // RCLCPP_INFO(this->get_logger(), "thrust_pwm is calculated to be %2.5f in pwm", thrust_pwm); + + auto crazy_vel_cmd = std::make_unique(); + + float e_yaw = yaw_des - yaw_cur; + if(e_yaw > M_PI) + e_yaw -= 2 * M_PI; + else if(e_yaw < -M_PI) + e_yaw += 2 * M_PI; + + float yaw_rate_des = ((-msg->kr[2] * e_yaw) - msg->angular_velocity.z) * (180 / M_PI); + + // TODO change this check to be a param + // throttle the publish rate + // if ((this->now() - last_so3_cmd_time_).seconds() > 1.0/30.0){ + crazy_vel_cmd->linear.y = roll_des + msg->aux.angle_corrections[0]; + crazy_vel_cmd->linear.x = pitch_des + msg->aux.angle_corrections[1]; + crazy_vel_cmd->linear.z = CLAMP(thrust_pwm, thrust_pwm_min_, thrust_pwm_max_); + + // RCLCPP_INFO(this->get_logger(), "commanded thrust is %2.2f", CLAMP(thrust_pwm, thrust_pwm_min_, thrust_pwm_max_)); + + crazy_vel_cmd->angular.z = yaw_rate_des; + // RCLCPP_INFO(this->get_logger(), "commanded yaw rate is %2.2f", yaw_rate_des); //yaw_debug + + crazy_fast_cmd_vel_pub_->publish(std::move(crazy_vel_cmd)); + // save last so3_cmd + last_so3_cmd_ = *msg; + // last_so3_cmd_time_ = this->now(); + last_so3_cmd_time_ = msg->header.stamp; + //} + // else { + // RCLCPP_INFO_STREAM(this->get_logger(), "Commands too quick, time since is: " << (this->now() - + // last_so3_cmd_time_).seconds()); + //} +} + +SO3CmdToCrazyflie::SO3CmdToCrazyflie(const rclcpp::NodeOptions &options) + : Node("so3cmd_to_crazyflie", rclcpp::NodeOptions(options).use_intra_process_comms(true)) +{ + this->declare_parameter("kp_yaw_rate", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("c1", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("c2", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("c3", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("so3_cmd_timeout", 0.1); + this->declare_parameter("thrust_pwm_min", 10000); + this->declare_parameter("thrust_pwm_max", 60000); + + // get thrust scaling parameters + // Note that this is ignoring a constant based on the number of props, which + // is captured with the lin_cof_a variable later. + // + // TODO this is where we load thrust scaling stuff + if(this->get_parameter("kp_yaw_rate", kp_yaw_rate_)) + RCLCPP_INFO(this->get_logger(), "kp yaw rate is %2.2f", kp_yaw_rate_); + else + RCLCPP_FATAL(this->get_logger(), "kp yaw rate not found"); + + // get thrust scaling parameters + if(this->get_parameter("c1", c1_) && this->get_parameter("c2", c2_) && this->get_parameter("c3", c3_)) + RCLCPP_INFO(this->get_logger(), "Using %2.2f, %2.2f, %2.2f for thrust mapping", c1_, c2_, c3_); + else + RCLCPP_FATAL(this->get_logger(), "Must set coefficients for thrust scaling"); + + // get param for so3 command timeout duration + so3_cmd_timeout_ = this->get_parameter("so3_cmd_timeout").as_double(); + + thrust_pwm_max_ = this->get_parameter("thrust_pwm_max").as_int(); + thrust_pwm_min_ = this->get_parameter("thrust_pwm_min").as_int(); + + odom_set_ = false; + so3_cmd_set_ = false; + motor_status_ = 0; + + // TODO make sure this is publishing to the right place + crazy_fast_cmd_vel_pub_ = this->create_publisher("~/cmd_vel_fast", 10); + + crazy_cmd_vel_pub_ = this->create_publisher("~/cmd_vel", 10); + + // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos1 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + so3_cmd_sub_ = this->create_subscription( + "~/so3_cmd", qos2, std::bind(&SO3CmdToCrazyflie::so3_cmd_callback, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + "~/odom", qos1, std::bind(&SO3CmdToCrazyflie::odom_callback, this, std::placeholders::_1)); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(SO3CmdToCrazyflie) diff --git a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp deleted file mode 100644 index ddc8bb67..00000000 --- a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp +++ /dev/null @@ -1,231 +0,0 @@ -// This nodelet is meant to subscribe to so3 commands and then convert them -// to geometry_msgs/Twist which is the type for cmd_vel, the crazyflie control -// topic. The format of this is as follows: -// linear.y = roll [-30 to 30 degrees] (may be negative) -// linear.x = pitch [-30 to 30 degrees] (may be negative) -// linear.z = thrust [0 to 60,000] (motors stiction around 2000) -// angular.z = yawrate [-200 to 200 degrees/second] (note this is not yaw!) - -#include -#include -#include -#include -#include - -#include - -// TODO: Remove CLAMP as macro -#define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x) - -class SO3CmdToCrazyflie : public nodelet::Nodelet -{ - public: - void onInit(void); - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - private: - void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg); - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - - bool odom_set_, so3_cmd_set_; - Eigen::Quaterniond q_odom_; - - ros::Publisher attitude_raw_pub_; - ros::Publisher crazy_fast_cmd_vel_pub_, crazy_cmd_vel_pub_; - - ros::Subscriber so3_cmd_sub_; - ros::Subscriber odom_sub_; - - double so3_cmd_timeout_; - ros::Time last_so3_cmd_time_; - kr_mav_msgs::SO3Command last_so3_cmd_; - - double c1_; - double c2_; - double c3_; - - // TODO get rid of this for the gains coming in - double kp_yaw_rate_; - - int thrust_pwm_min_; // Necessary to overcome stiction - int thrust_pwm_max_; // Mapped to PWM - - int motor_status_; -}; - -void SO3CmdToCrazyflie::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) -{ - if(!odom_set_) - odom_set_ = true; - - q_odom_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, - odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); - - if(so3_cmd_set_ && ((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_)) - { - // ROS_WARN("so3_cmd timeout. %f seconds since last command", - // (ros::Time::now() - last_so3_cmd_time_).toSec()); - const auto last_so3_cmd_ptr = boost::make_shared(last_so3_cmd_); - - so3_cmd_callback(last_so3_cmd_ptr); - } -} - -void SO3CmdToCrazyflie::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg) -{ - if(!so3_cmd_set_) - so3_cmd_set_ = true; - - // switch on motors - if(msg->aux.enable_motors) - { - // If the crazyflie motors are timed out, we need to send a zero message in order to get them to start - if(motor_status_ < 3) - { - geometry_msgs::Twist::Ptr motors_vel_cmd(new geometry_msgs::Twist); - crazy_cmd_vel_pub_.publish(motors_vel_cmd); - last_so3_cmd_ = *msg; - last_so3_cmd_time_ = msg->header.stamp; - motor_status_ += 1; - return; - } - // After sending zero message send min thrust - if(motor_status_ < 10) - { - geometry_msgs::Twist::Ptr motors_vel_cmd(new geometry_msgs::Twist); - motors_vel_cmd->linear.z = thrust_pwm_min_; - crazy_cmd_vel_pub_.publish(motors_vel_cmd); - } - motor_status_ += 1; - } - else if(!msg->aux.enable_motors) - { - motor_status_ = 0; - geometry_msgs::Twist::Ptr motors_vel_cmd(new geometry_msgs::Twist); - crazy_cmd_vel_pub_.publish(motors_vel_cmd); - last_so3_cmd_ = *msg; - last_so3_cmd_time_ = msg->header.stamp; - return; - } - - /* - // If the crazyflie motors are timed out, we need to send a zero message in order to get them to start - if((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_){ - crazy_cmd_vel_pub_.publish(crazy_vel_cmd); - // Keep in mind that this publishes two messages in a row, this may cause bandwith problems - } - */ - - // 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); - - // check psi for stability - const Eigen::Matrix3d R_des(q_des); - const Eigen::Matrix3d R_cur(q_odom_); - - const float yaw_cur = std::atan2(R_cur(1, 0), R_cur(0, 0)); - const float yaw_des = std::atan2(R_des(1, 0), R_des(0, 0)); - - // Map these into the current body frame (based on yaw) - Eigen::Matrix3d R_des_new = R_des * Eigen::AngleAxisd(yaw_cur - yaw_des, Eigen::Vector3d::UnitZ()); - float pitch_des = -std::asin(R_des_new(2, 0)); - float roll_des = std::atan2(R_des_new(2, 1), R_des_new(2, 2)); - - roll_des = roll_des * 180 / M_PI; - pitch_des = pitch_des * 180 / M_PI; - - double thrust_f = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); // Force in Newtons - // ROS_INFO("thrust_f is %2.5f newtons", thrust_f); - thrust_f = std::max(thrust_f, 0.0); - - thrust_f = thrust_f * 1000 / 9.81; // Force in grams - // ROS_INFO("thrust_f is %2.5f grams", thrust_f); - - // ROS_INFO("coeffs are %2.2f, %2.2f, %2.2f", c1_, c2_, c3_); - float thrust_pwm = c1_ + c2_ * std::sqrt(c3_ + thrust_f); - - // ROS_INFO("thrust_pwm is %2.5f from 0-1", thrust_pwm); - - thrust_pwm = thrust_pwm * thrust_pwm_max_; // thrust_pwm mapped from 0-60000 - // ROS_INFO("thrust_pwm is calculated to be %2.5f in pwm", thrust_pwm); - - geometry_msgs::Twist::Ptr crazy_vel_cmd(new geometry_msgs::Twist); - - float e_yaw = yaw_des - yaw_cur; - if(e_yaw > M_PI) - e_yaw -= 2 * M_PI; - else if(e_yaw < -M_PI) - e_yaw += 2 * M_PI; - - float yaw_rate_des = ((-msg->kR[2] * e_yaw) - msg->angular_velocity.z) * (180 / M_PI); - - // TODO change this check to be a param - // throttle the publish rate - // if ((ros::Time::now() - last_so3_cmd_time_).toSec() > 1.0/30.0){ - crazy_vel_cmd->linear.y = roll_des + msg->aux.angle_corrections[0]; - crazy_vel_cmd->linear.x = pitch_des + msg->aux.angle_corrections[1]; - crazy_vel_cmd->linear.z = CLAMP(thrust_pwm, thrust_pwm_min_, thrust_pwm_max_); - - // ROS_INFO("commanded thrust is %2.2f", CLAMP(thrust_pwm, thrust_pwm_min_, thrust_pwm_max_)); - - crazy_vel_cmd->angular.z = yaw_rate_des; - // ROS_INFO("commanded yaw rate is %2.2f", yaw_rate_des); //yaw_debug - - crazy_fast_cmd_vel_pub_.publish(crazy_vel_cmd); - // save last so3_cmd - last_so3_cmd_ = *msg; - // last_so3_cmd_time_ = ros::Time::now(); - last_so3_cmd_time_ = msg->header.stamp; - //} - // else { - // ROS_INFO_STREAM("Commands too quick, time since is: " << (ros::Time::now() - last_so3_cmd_time_).toSec()); - //} -} - -void SO3CmdToCrazyflie::onInit(void) -{ - ros::NodeHandle priv_nh(getPrivateNodeHandle()); - - // get thrust scaling parameters - // Note that this is ignoring a constant based on the number of props, which - // is captured with the lin_cof_a variable later. - // - // TODO this is where we load thrust scaling stuff - if(priv_nh.getParam("kp_yaw_rate", kp_yaw_rate_)) - ROS_INFO("kp yaw rate is %2.2f", kp_yaw_rate_); - else - ROS_FATAL("kp yaw rate not found"); - - // get thrust scaling parameters - if(priv_nh.getParam("c1", c1_) && priv_nh.getParam("c2", c2_) && priv_nh.getParam("c3", c3_)) - ROS_INFO("Using %2.2f, %2.2f, %2.2f for thrust mapping", c1_, c2_, c3_); - else - ROS_FATAL("Must set coefficients for thrust scaling"); - - // get param for so3 command timeout duration - priv_nh.param("so3_cmd_timeout", so3_cmd_timeout_, 0.1); - - priv_nh.param("thrust_pwm_max", thrust_pwm_max_, 60000); - priv_nh.param("thrust_pwm_min", thrust_pwm_min_, 10000); - - odom_set_ = false; - so3_cmd_set_ = false; - motor_status_ = 0; - - // TODO make sure this is publishing to the right place - crazy_fast_cmd_vel_pub_ = priv_nh.advertise("cmd_vel_fast", 10); - - crazy_cmd_vel_pub_ = priv_nh.advertise("cmd_vel", 10); - - so3_cmd_sub_ = - priv_nh.subscribe("so3_cmd", 1, &SO3CmdToCrazyflie::so3_cmd_callback, this, ros::TransportHints().tcpNoDelay()); - - odom_sub_ = - priv_nh.subscribe("odom", 10, &SO3CmdToCrazyflie::odom_callback, this, ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(SO3CmdToCrazyflie, nodelet::Nodelet); diff --git a/rqt_mav_manager/CMakeLists.txt b/rqt_mav_manager/CMakeLists.txt deleted file mode 100644 index 37663e2a..00000000 --- a/rqt_mav_manager/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(rqt_mav_manager) - -find_package(catkin REQUIRED) -catkin_python_setup() - -catkin_package() - -install(FILES plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -install(DIRECTORY resource DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -install(PROGRAMS scripts/rqt_mav_manager DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/rqt_mav_manager/package.xml b/rqt_mav_manager/package.xml index f0a65749..e434bbb8 100644 --- a/rqt_mav_manager/package.xml +++ b/rqt_mav_manager/package.xml @@ -1,25 +1,29 @@ - + + + rqt_mav_manager - 0.1.0 - Provides a GUI plugin for MAV Manager. - Dinesh Thakur - Kartik Mohta + 0.0.0 + TODO: Package description + pranav + TODO: License declaration - BSD + rclpy - https://github.com/KumarRobotics/kr_ui - - Dinesh Thakur - - catkin - - rospy + ament_index_python + python3-pyside2 + python3-qt5-bindings + python_qt_binding + rqt_gui rqt_gui_py - kr_mav_manager + qt_gui_py_common std_srvs - rqt_gui + example_interfaces + kr_mav_manager + + python3-pytest + ament_python diff --git a/rqt_mav_manager/resource/rqt_mav_manager b/rqt_mav_manager/resource/rqt_mav_manager new file mode 100644 index 00000000..e69de29b diff --git a/rqt_mav_manager/rqt_mav_manager/__init__.py b/rqt_mav_manager/rqt_mav_manager/__init__.py new file mode 100644 index 00000000..cddef78d --- /dev/null +++ b/rqt_mav_manager/rqt_mav_manager/__init__.py @@ -0,0 +1,273 @@ +#!/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 rqt_gui_py.plugin import Plugin +from ament_index_python import get_resource + +import kr_mav_manager.srv +import std_srvs.srv +from example_interfaces.srv import AddTwoInts + +class MavManagerUi(Plugin): + + def __init__(self, context): + super().__init__(context) + self.setObjectName('MavManagerUi') + self._context = context + + self._context.node.declare_parameter('robot_name', "quadrotor") + + self.robot_name = self._context.node.get_parameter('robot_name').value + self.mav_node_name = 'mav_services' + + self._widget = QWidget() + _, package_path = get_resource("packages", "rqt_mav_manager") + ui_file = os.path.join(package_path, 'share', 'rqt_mav_manager', 'resource', 'MavManager.ui') + loadUi(ui_file, self._widget) + self._widget.setObjectName('MAVManagerWidget') + + if self._context.serial_number() > 1: + self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) + self._context.add_widget(self._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) + + self._widget.motors_on_push_button.pressed.connect(self._on_motors_on_pressed) + self._widget.motors_off_push_button.pressed.connect(self._on_motors_off_pressed) + self._widget.hover_push_button.pressed.connect(self._on_hover_pressed) + self._widget.ehover_push_button.pressed.connect(self._on_ehover_pressed) + self._widget.land_push_button.pressed.connect(self._on_land_pressed) + self._widget.eland_push_button.pressed.connect(self._on_eland_pressed) + self._widget.estop_push_button.pressed.connect(self._on_estop_pressed) + self._widget.goto_push_button.pressed.connect(self._on_goto_pressed) + + self._widget.takeoff_push_button.pressed.connect(self._on_takeoff_pressed) + self._widget.gohome_push_button.pressed.connect(self._on_gohome_pressed) + + def _on_robot_name_changed(self, robot_name): + self.robot_name = str(robot_name) + + def _on_node_name_changed(self, node_name): + self.mav_node_name = str(node_name) + + def _on_motors_on_pressed(self): + motors_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/motors' + client = self._context.node.create_client(std_srvs.srv.SetBool, motors_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {motors_topic} not available") + return + + request = std_srvs.srv.SetBool.Request() + request.data = True + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('Motors on: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_motors_off_pressed(self): + motors_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/motors' + client = self._context.node.create_client(std_srvs.srv.SetBool, motors_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {motors_topic} not available") + return + + request = std_srvs.srv.SetBool.Request() + request.data = False + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('Motors off: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_hover_pressed(self): + hover_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/hover' + client = self._context.node.create_client(std_srvs.srv.Trigger, hover_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {hover_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('Hover: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_ehover_pressed(self): + ehover_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/ehover' + client = self._context.node.create_client(std_srvs.srv.Trigger, ehover_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {ehover_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('EHover: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_land_pressed(self): + land_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/land' + client = self._context.node.create_client(std_srvs.srv.Trigger, land_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {land_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('Land: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_eland_pressed(self): + eland_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/eland' + client = self._context.node.create_client(std_srvs.srv.Trigger, eland_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {eland_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('ELand: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_estop_pressed(self): + estop_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/estop' + client = self._context.node.create_client(std_srvs.srv.Trigger, estop_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {estop_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('EStop: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_takeoff_pressed(self): + takeoff_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/takeoff' + client = self._context.node.create_client(std_srvs.srv.Trigger, takeoff_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {takeoff_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('Takeoff: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_gohome_pressed(self): + gohome_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/goHome' + client = self._context.node.create_client(std_srvs.srv.Trigger, gohome_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {gohome_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('goHome: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_goto_pressed(self): + request = kr_mav_manager.srv.Vec4.Request() + request.goal[0] = self._widget.x_doubleSpinBox.value() + request.goal[1] = self._widget.y_doubleSpinBox.value() + request.goal[2] = self._widget.z_doubleSpinBox.value() + request.goal[3] = self._widget.yaw_doubleSpinBox.value() + + print(request.goal) + + if(self._widget.relative_checkbox.isChecked()): + goto_relative = '/'+self.robot_name+'/'+self.mav_node_name+'/goToRelative' + client = self._context.node.create_client(kr_mav_manager.srv.Vec4, goto_relative) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {goto_relative} not available") + return + + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('goToRelative: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + else: + goto = '/'+self.robot_name+'/'+self.mav_node_name+'/goTo' + client = self._context.node.create_client(kr_mav_manager.srv.Vec4, goto) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {goto} not available") + return + + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('goTo: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + + # Qt Methods + def shutdown_plugin(self): + return super().shutdown_plugin() + + def save_settings(self, plugin_settings, instance_settings): + instance_settings.set_value('robot_name', self._widget.robot_name_line_edit.text()) + instance_settings.set_value('node_name' , self._widget.node_name_line_edit.text()) + + def restore_settings(self, plugin_settings, instance_settings): + + #Override saved value with param value if set + param_value = self._context.node.get_parameter('robot_name').value + self.robot_name = param_value + self._widget.robot_name_line_edit.setText(param_value) + + value = instance_settings.value('node_name', "mav_services") + self.node_name = value + self._widget.node_name_line_edit.setText(value) + diff --git a/rqt_mav_manager/rqt_mav_manager/rqt_mav_manager.py b/rqt_mav_manager/rqt_mav_manager/rqt_mav_manager.py new file mode 100644 index 00000000..7275338a --- /dev/null +++ b/rqt_mav_manager/rqt_mav_manager/rqt_mav_manager.py @@ -0,0 +1,13 @@ +#!/usr/bin/env python3 + +import sys + +from rqt_gui.main import Main + +def main(): +# Run the plugin + main = Main() + sys.exit(main.main(sys.argv, standalone="rqt_mav_manager.MavManagerUi")) + +if __name__ == "__main__": + main() diff --git a/rqt_mav_manager/scripts/rqt_mav_manager b/rqt_mav_manager/scripts/rqt_mav_manager deleted file mode 100755 index 44043283..00000000 --- a/rqt_mav_manager/scripts/rqt_mav_manager +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python3 - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_mav_manager.MavManagerUi')) diff --git a/rqt_mav_manager/setup.cfg b/rqt_mav_manager/setup.cfg new file mode 100644 index 00000000..9037326c --- /dev/null +++ b/rqt_mav_manager/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rqt_mav_manager +[install] +install_scripts=$base/lib/rqt_mav_manager diff --git a/rqt_mav_manager/setup.py b/rqt_mav_manager/setup.py index 6268a9c3..d58e062a 100644 --- a/rqt_mav_manager/setup.py +++ b/rqt_mav_manager/setup.py @@ -1,9 +1,28 @@ -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import find_packages, setup -d = generate_distutils_setup( - packages=['rqt_mav_manager'], - package_dir={'': 'src'}, -) +package_name = 'rqt_mav_manager' -setup(**d) +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name + '/resource', ['resource/MavManager.ui']), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, ['plugin.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Pranav Shah', + maintainer_email='pranavpshah2098@gmail.com', + description='TODO: Package description', + license='BSD', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + "rqt_mav_manager = rqt_mav_manager.rqt_mav_manager:main" + ], + }, +) diff --git a/rqt_mav_manager/src/rqt_mav_manager/__init__.py b/rqt_mav_manager/src/rqt_mav_manager/__init__.py deleted file mode 100644 index 85665c2a..00000000 --- a/rqt_mav_manager/src/rqt_mav_manager/__init__.py +++ /dev/null @@ -1,211 +0,0 @@ -from __future__ import print_function - -import os -import rospkg - -import rospy -from python_qt_binding import loadUi -from python_qt_binding.QtWidgets import QWidget -from rqt_gui_py.plugin import Plugin - -import kr_mav_manager.srv -import std_srvs.srv - -class MavManagerUi(Plugin): - - def __init__(self, context): - super(MavManagerUi, self).__init__(context) - self.setObjectName('MavManagerUi') - - self._publisher = None - - self.robot_name = 'quadrotor' - self.mav_node_name = 'mav_services' - - self._widget = QWidget() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_mav_manager'), 'resource', 'MavManager.ui') - loadUi(ui_file, self._widget) - self._widget.setObjectName('MavManagerWidget') - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._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) - - self._widget.motors_on_push_button.pressed.connect(self._on_motors_on_pressed) - self._widget.motors_off_push_button.pressed.connect(self._on_motors_off_pressed) - self._widget.hover_push_button.pressed.connect(self._on_hover_pressed) - self._widget.ehover_push_button.pressed.connect(self._on_ehover_pressed) - self._widget.land_push_button.pressed.connect(self._on_land_pressed) - self._widget.eland_push_button.pressed.connect(self._on_eland_pressed) - self._widget.estop_push_button.pressed.connect(self._on_estop_pressed) - self._widget.goto_push_button.pressed.connect(self._on_goto_pressed) - - self._widget.takeoff_push_button.pressed.connect(self._on_takeoff_pressed) - self._widget.gohome_push_button.pressed.connect(self._on_gohome_pressed) - - def _on_robot_name_changed(self, robot_name): - self.robot_name = str(robot_name) - - def _on_node_name_changed(self, node_name): - self.mav_node_name = str(node_name) - - def _on_motors_on_pressed(self): - try: - motors_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/motors' - rospy.wait_for_service(motors_topic, timeout=1.0) - motors_on = rospy.ServiceProxy(motors_topic, std_srvs.srv.SetBool) - resp = motors_on(True) - print('Motors on ', resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_motors_off_pressed(self): - try: - motors_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/motors' - rospy.wait_for_service(motors_topic, timeout=1.0) - motors_off = rospy.ServiceProxy(motors_topic, std_srvs.srv.SetBool) - resp = motors_off(False) - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_hover_pressed(self): - try: - hover_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/hover' - rospy.wait_for_service(hover_topic, timeout=1.0) - hover = rospy.ServiceProxy(hover_topic, std_srvs.srv.Trigger) - resp = hover() - print('Hover ', resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_ehover_pressed(self): - try: - ehover_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/ehover' - rospy.wait_for_service(ehover_topic, timeout=1.0) - ehover = rospy.ServiceProxy(ehover_topic, std_srvs.srv.Trigger) - resp = ehover() - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_land_pressed(self): - try: - land_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/land' - rospy.wait_for_service(land_topic, timeout=1.0) - land = rospy.ServiceProxy(land_topic, std_srvs.srv.Trigger) - resp = land() - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_eland_pressed(self): - try: - eland_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/eland' - rospy.wait_for_service(eland_topic, timeout=1.0) - eland = rospy.ServiceProxy(eland_topic, std_srvs.srv.Trigger) - resp = eland() - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_estop_pressed(self): - try: - estop_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/estop' - rospy.wait_for_service(estop_topic, timeout=1.0) - estop = rospy.ServiceProxy(estop_topic, std_srvs.srv.Trigger) - resp = estop() - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_takeoff_pressed(self): - try: - takeoff_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/takeoff' - rospy.wait_for_service(takeoff_topic, timeout=1.0) - takeoff = rospy.ServiceProxy(takeoff_topic, std_srvs.srv.Trigger) - resp = takeoff() - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_gohome_pressed(self): - try: - gohome_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/goHome' - rospy.wait_for_service(gohome_topic, timeout=1.0) - gohome = rospy.ServiceProxy(gohome_topic, std_srvs.srv.Trigger) - resp = gohome() - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_goto_pressed(self): - - req = kr_mav_manager.srv.Vec4Request() - - req.goal[0] = self._widget.x_doubleSpinBox.value() - req.goal[1] = self._widget.y_doubleSpinBox.value() - req.goal[2] = self._widget.z_doubleSpinBox.value() - req.goal[3] = self._widget.yaw_doubleSpinBox.value() - - print(req.goal) - - if(self._widget.relative_checkbox.isChecked()): - try: - goto = rospy.ServiceProxy('/'+self.robot_name+'/'+self.mav_node_name+'/goToRelative', kr_mav_manager.srv.Vec4) - resp = goto(req) - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - else: - try: - goto_relative = rospy.ServiceProxy('/'+self.robot_name+'/'+self.mav_node_name+'/goTo', kr_mav_manager.srv.Vec4) - resp = goto_relative(req) - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - - def _unregister_publisher(self): - if self._publisher is not None: - self._publisher.unregister() - self._publisher = None - - def shutdown_plugin(self): - self._unregister_publisher() - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('robot_name' , self._widget.robot_name_line_edit.text()) - instance_settings.set_value('node_name' , self._widget.node_name_line_edit.text()) - - def restore_settings(self, plugin_settings, instance_settings): - - #Override saved value with param value if set - value = instance_settings.value('robot_name', "quadrotor") - param_value = rospy.get_param("robot_name", value) - self.robot_name = param_value - self._widget.robot_name_line_edit.setText(param_value) - - value = instance_settings.value('node_name', "mav_services") - self.node_name = value - self._widget.node_name_line_edit.setText(value) From f9962e9828ef69a8f007c8913a01ab713dc3ae2d Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Mon, 25 Mar 2024 16:04:52 -0400 Subject: [PATCH 13/64] update maintainers --- rqt_mav_manager/package.xml | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rqt_mav_manager/package.xml b/rqt_mav_manager/package.xml index e434bbb8..fe029513 100644 --- a/rqt_mav_manager/package.xml +++ b/rqt_mav_manager/package.xml @@ -3,9 +3,12 @@ rqt_mav_manager 0.0.0 - TODO: Package description + Provides a GUI plugin for MAV Manager. + Dinesh Thakur + Kartik Mohta pranav - TODO: License declaration + + BSD rclpy From 39ae768c8b0790e677340b50d9066f7c48dd5146 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Wed, 27 Mar 2024 16:09:21 -0400 Subject: [PATCH 14/64] update version to 0.2.0 --- rqt_mav_manager/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rqt_mav_manager/package.xml b/rqt_mav_manager/package.xml index fe029513..3de8618c 100644 --- a/rqt_mav_manager/package.xml +++ b/rqt_mav_manager/package.xml @@ -2,7 +2,7 @@ rqt_mav_manager - 0.0.0 + 0.2.0 Provides a GUI plugin for MAV Manager. Dinesh Thakur Kartik Mohta From 8b8e6bb80884a0083d7aa1a08565c192c653da38 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Sat, 5 Oct 2024 11:00:27 -0500 Subject: [PATCH 15/64] kr_trackers ros2 humble compatible --- trackers/kr_trackers/CMakeLists.txt | 117 +++---- trackers/kr_trackers/README.md | 21 ++ .../include/kr_trackers/Tracker.hpp | 56 +++ ...al_conditions.h => initial_conditions.hpp} | 13 +- .../include/kr_trackers/lissajous_generator.h | 2 + .../include/kr_trackers/traj_gen.h | 2 + trackers/kr_trackers/nodelet_plugin.xml | 71 ---- trackers/kr_trackers/package.xml | 26 +- trackers/kr_trackers/plugins_description.xml | 15 + .../scripts/twist_to_velocity_goal.py | 2 + .../scripts/waypoints_to_action.py | 2 + .../kr_trackers/src/circle_tracker_server.cpp | 2 + .../kr_trackers/src/initial_conditions.cpp | 13 +- .../src/line_tracker_distance_server.cpp | 321 +++++++++++------- .../src/line_tracker_min_jerk_server.cpp | 296 +++++++++------- .../src/line_tracker_trapezoid.cpp | 2 + trackers/kr_trackers/src/line_tracker_yaw.cpp | 2 + .../src/lissajous_adder_server.cpp | 2 + .../kr_trackers/src/lissajous_generator.cpp | 2 + .../src/lissajous_tracker_server.cpp | 2 + trackers/kr_trackers/src/null_tracker.cpp | 2 + .../src/smooth_vel_tracker_server.cpp | 2 + .../kr_trackers/src/trajectory_tracker.cpp | 2 + trackers/kr_trackers/src/velocity_tracker.cpp | 2 + 24 files changed, 574 insertions(+), 403 deletions(-) create mode 100644 trackers/kr_trackers/README.md create mode 100644 trackers/kr_trackers/include/kr_trackers/Tracker.hpp rename trackers/kr_trackers/include/kr_trackers/{initial_conditions.h => initial_conditions.hpp} (58%) delete mode 100644 trackers/kr_trackers/nodelet_plugin.xml create mode 100644 trackers/kr_trackers/plugins_description.xml diff --git a/trackers/kr_trackers/CMakeLists.txt b/trackers/kr_trackers/CMakeLists.txt index 98c2751c..9b60dfb3 100644 --- a/trackers/kr_trackers/CMakeLists.txt +++ b/trackers/kr_trackers/CMakeLists.txt @@ -1,76 +1,71 @@ cmake_minimum_required(VERSION 3.10) project(kr_trackers) -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcutils REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(kr_mav_msgs REQUIRED) +find_package(kr_tracker_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(pluginlib REQUIRED) -find_package( - catkin REQUIRED - COMPONENTS actionlib - std_msgs - geometry_msgs - nav_msgs - tf - kr_mav_msgs - kr_tracker_msgs - kr_trackers_manager) -find_package(Eigen3 REQUIRED) - -catkin_package( - INCLUDE_DIRS +include_directories( include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - actionlib - std_msgs - geometry_msgs - nav_msgs - tf +) + +set(plugins_dependencies + pluginlib +) + +set(node_dependencies + ${plugins_dependencies} + rclcpp + rcutils + rclcpp_components + rclcpp_action + rclcpp_lifecycle kr_mav_msgs kr_tracker_msgs - kr_trackers_manager - DEPENDS - EIGEN3) + nav_msgs + tf2_geometry_msgs + tf2 +) -add_library( - ${PROJECT_NAME} - src/initial_conditions.cpp - src/circle_tracker_server.cpp - src/initial_conditions.cpp +add_library(${PROJECT_NAME}_plugins SHARED src/line_tracker_distance_server.cpp - src/line_tracker_min_jerk_server.cpp - # src/line_tracker_trapezoid.cpp - # src/line_tracker_yaw.cpp - src/lissajous_adder_server.cpp - src/lissajous_generator.cpp - src/lissajous_tracker_server.cpp - src/null_tracker.cpp - src/smooth_vel_tracker_server.cpp - src/trajectory_tracker.cpp - src/traj_gen.cpp - src/velocity_tracker.cpp) -target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} Eigen3::Eigen) + src/line_tracker_min_jerk_server.cpp + src/initial_conditions.cpp) +ament_target_dependencies(${PROJECT_NAME}_plugins ${node_dependencies}) + -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install(TARGETS + ${PROJECT_NAME}_plugins + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib +) -install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(DIRECTORY include/ + DESTINATION include/ +) -install(PROGRAMS - scripts/twist_to_velocity_goal.py - scripts/waypoints_to_action.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(FILES plugins_description.xml + DESTINATION share ) + + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}_plugins) +pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins_description.xml) +ament_package() diff --git a/trackers/kr_trackers/README.md b/trackers/kr_trackers/README.md new file mode 100644 index 00000000..29ec0616 --- /dev/null +++ b/trackers/kr_trackers/README.md @@ -0,0 +1,21 @@ +kr_trackers +============= + +Package for Trackers + +## Migration of files to ROS2 compatible format +- [ ] circle_tracker_server.cpp +- [x] initial_conditions.cpp +- [x] line_tracker_distance_server.cpp +- [x] line_tracker_min_jerk_server.cpp +- [ ] line_tracker_trapezoid.cpp +- [ ] line_tracker_yaw.cpp +- [ ] lissajous_adder_server.cpp +- [ ] lissajous_generator.cpp +- [ ] lissajous_tracker_server.cpp +- [ ] null_tracker.cpp +- [ ] smooth_vel_tracker_server.cpp +- [ ] taj_gen.cpp +- [ ] tracjectory_tracker.cpp +- [ ] velocity_tracker.cpp + diff --git a/trackers/kr_trackers/include/kr_trackers/Tracker.hpp b/trackers/kr_trackers/include/kr_trackers/Tracker.hpp new file mode 100644 index 00000000..4080f7fa --- /dev/null +++ b/trackers/kr_trackers/include/kr_trackers/Tracker.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include "kr_mav_msgs/msg/position_command.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace kr_trackers_manager +{ +class Tracker +{ + public: + virtual ~Tracker(void) {} + + /** + * @brief Initialize the tracker. Should be used to get the params, construct the publishers and subscribers. + * + * @param parent pointer to the kr_trackers_manager node and can be used to read params and contruct action servers for trackers + */ + virtual void Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) = 0; + + /** + * @brief Activate the tracker. This indicates that the tracker should get ready to publish commands. + * + * @param cmd The last PositionCommand that was published, can be used to maintain continuity of commands when + * switching trackers. + * + * @return Should return true if the tracker is ready to publish commands, else return false. + */ + virtual bool Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) = 0; + + /** + * @brief Deactivate the tracker. This is called when the kr_trackers_manager switches to another tracker. + */ + virtual void Deactivate(void) = 0; + + /** + * @brief Get the current command output from the tracker. + * Note that this function is still called even if the tracker has not been activated. This is for cases when the + * tracker would want to use the previous robot odometry to compute current commands. + * + * @param msg The current odometry message which should be used by the tracker to generate the command. + * + * @return The PositionCommand message which would be published. If an uninitialized ConstSharedPtr is returned, then no + * PositionCommand message would be published. + */ + virtual kr_mav_msgs::msg::PositionCommand::ConstSharedPtr update(const nav_msgs::msg::Odometry::SharedPtr msg) = 0; + + /** + * @brief Get status of the tracker. Only called when the tracker has been activated. + * + * @return The tracker status (see the options in the TrackerStatus message). + */ + virtual uint8_t status() = 0; +}; + +} // namespace kr_trackers_manager diff --git a/trackers/kr_trackers/include/kr_trackers/initial_conditions.h b/trackers/kr_trackers/include/kr_trackers/initial_conditions.hpp similarity index 58% rename from trackers/kr_trackers/include/kr_trackers/initial_conditions.h rename to trackers/kr_trackers/include/kr_trackers/initial_conditions.hpp index ebff2281..4606c336 100644 --- a/trackers/kr_trackers/include/kr_trackers/initial_conditions.h +++ b/trackers/kr_trackers/include/kr_trackers/initial_conditions.hpp @@ -1,8 +1,7 @@ -#ifndef STD_TRACKERS_INITIAL_CONDITIONS_H -#define STD_TRACKERS_INITIAL_CONDITIONS_H +#pragma once -#include -#include +#include "kr_mav_msgs/msg/position_command.hpp" +#include "nav_msgs/msg/odometry.hpp" #include @@ -11,8 +10,8 @@ class InitialConditions public: InitialConditions(); - void set_from_cmd(const kr_mav_msgs::PositionCommand::ConstPtr &msg); - void set_from_odom(const nav_msgs::Odometry::ConstPtr &msg); + void set_from_cmd(const kr_mav_msgs::msg::PositionCommand::SharedPtr msg); + void set_from_odom(const nav_msgs::msg::Odometry::SharedPtr msg); Eigen::Vector3f pos() const { return pos_; } Eigen::Vector3f vel() const { return vel_; } Eigen::Vector3f acc() const { return acc_; } @@ -26,5 +25,3 @@ class InitialConditions float yaw_, yaw_dot_; bool cmd_valid_; }; - -#endif // STD_TRACKERS_INITIAL_CONDITIONS_H diff --git a/trackers/kr_trackers/include/kr_trackers/lissajous_generator.h b/trackers/kr_trackers/include/kr_trackers/lissajous_generator.h index dd2b7620..02a75ad3 100644 --- a/trackers/kr_trackers/include/kr_trackers/lissajous_generator.h +++ b/trackers/kr_trackers/include/kr_trackers/lissajous_generator.h @@ -1,3 +1,5 @@ +// TODO: convert to hpp and compatible with ros2 + #ifndef _LISSAJOUS_GENERATOR_H_ #define _LISSAJOUS_GENERATOR_H_ diff --git a/trackers/kr_trackers/include/kr_trackers/traj_gen.h b/trackers/kr_trackers/include/kr_trackers/traj_gen.h index 0d34e3a9..7b0f6666 100644 --- a/trackers/kr_trackers/include/kr_trackers/traj_gen.h +++ b/trackers/kr_trackers/include/kr_trackers/traj_gen.h @@ -1,3 +1,5 @@ +// TODO: convert to hpp file and compatible with ros2 + #pragma once #include diff --git a/trackers/kr_trackers/nodelet_plugin.xml b/trackers/kr_trackers/nodelet_plugin.xml deleted file mode 100644 index 303a6157..00000000 --- a/trackers/kr_trackers/nodelet_plugin.xml +++ /dev/null @@ -1,71 +0,0 @@ - - - - - This is a tracker which follows a line from the current position to a given goal position using min jerk trajectory. - - - - - - This is a tracker which follows a line from the current position to a given goal position using a trapezoidal trajectory with distance along the trajectory as the parameter instead of time. - - - - - - This tracker literally does nothing. - - - - - - This tracker commands a desired velocity for the robot. - - - - - - This tracker goes from point A to point B trying to maintain constant velocity with a smooth ramp up. - - - - - - This tracker commands the robot to fly in a circle of a given size and period. - - - - - - This tracker commands the robot to follow a smooth trajectory passing through multiple waypoints. - - - - - - Follows a complex 3D Lissajous tracjectory - - - - - - Follows a sum of two Lissajous trajectories - - - - - - diff --git a/trackers/kr_trackers/package.xml b/trackers/kr_trackers/package.xml index 6ad75391..c6ab0203 100644 --- a/trackers/kr_trackers/package.xml +++ b/trackers/kr_trackers/package.xml @@ -1,28 +1,34 @@ - + + kr_trackers - 1.0.0 + 2.0.0 kr_trackers Kartik Mohta Michael Watterson Justin Thomas + Pranav Shah BSD Kartik Mohta - catkin + ament_cmake - actionlib - std_msgs - geometry_msgs - nav_msgs - tf + rclcpp + rclcpp_action + rclcpp_lifecycle + rclcpp_components kr_mav_msgs kr_tracker_msgs - kr_trackers_manager + nav_msgs + geometry_msgs + pluginlib + tf2 + tf2_geometry_msgs + rcutils - + ament_cmake diff --git a/trackers/kr_trackers/plugins_description.xml b/trackers/kr_trackers/plugins_description.xml new file mode 100644 index 00000000..e020a400 --- /dev/null +++ b/trackers/kr_trackers/plugins_description.xml @@ -0,0 +1,15 @@ + + + + + This is a tracker which follows a line from the current position to a given goal position using a trapezoidal trajectory with distance along the trajectory as the parameter instead of time. + + + + This is a tracker which follows a line from the current position to a given goal position using min jerk trajectory. + + + + diff --git a/trackers/kr_trackers/scripts/twist_to_velocity_goal.py b/trackers/kr_trackers/scripts/twist_to_velocity_goal.py index a7ac4532..68f50690 100755 --- a/trackers/kr_trackers/scripts/twist_to_velocity_goal.py +++ b/trackers/kr_trackers/scripts/twist_to_velocity_goal.py @@ -1,3 +1,5 @@ +# TODO: convert to ros2 compatible format + #! /usr/bin/env python3 from __future__ import print_function diff --git a/trackers/kr_trackers/scripts/waypoints_to_action.py b/trackers/kr_trackers/scripts/waypoints_to_action.py index 7b8464d5..2d1677d2 100755 --- a/trackers/kr_trackers/scripts/waypoints_to_action.py +++ b/trackers/kr_trackers/scripts/waypoints_to_action.py @@ -1,3 +1,5 @@ +# TODO: convert to ros2 compatible format + #! /usr/bin/env python3 from __future__ import print_function diff --git a/trackers/kr_trackers/src/circle_tracker_server.cpp b/trackers/kr_trackers/src/circle_tracker_server.cpp index fd2c1606..4b784de0 100644 --- a/trackers/kr_trackers/src/circle_tracker_server.cpp +++ b/trackers/kr_trackers/src/circle_tracker_server.cpp @@ -3,6 +3,8 @@ * 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 diff --git a/trackers/kr_trackers/src/initial_conditions.cpp b/trackers/kr_trackers/src/initial_conditions.cpp index e30f4ba3..d4303466 100644 --- a/trackers/kr_trackers/src/initial_conditions.cpp +++ b/trackers/kr_trackers/src/initial_conditions.cpp @@ -12,8 +12,9 @@ * in the desired, which we want to avoid. */ -#include -#include +#include "kr_trackers/initial_conditions.hpp" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" InitialConditions::InitialConditions() : pos_(Eigen::Vector3f::Zero()), @@ -26,11 +27,11 @@ InitialConditions::InitialConditions() { } -void InitialConditions::set_from_cmd(const kr_mav_msgs::PositionCommand::ConstPtr &msg) +void InitialConditions::set_from_cmd(const kr_mav_msgs::msg::PositionCommand::SharedPtr msg) { if(msg == NULL) { - ROS_WARN("Null PositionCommand recieved. Not setting initial condition."); + std::cout << "Null PositionCommand recieved. Not setting initial condition.\n"; return; } @@ -44,7 +45,7 @@ void InitialConditions::set_from_cmd(const kr_mav_msgs::PositionCommand::ConstPt cmd_valid_ = true; } -void InitialConditions::set_from_odom(const nav_msgs::Odometry::ConstPtr &msg) +void InitialConditions::set_from_odom(const nav_msgs::msg::Odometry::SharedPtr msg) { if(!cmd_valid_) { @@ -52,7 +53,7 @@ void InitialConditions::set_from_odom(const nav_msgs::Odometry::ConstPtr &msg) vel_ = Eigen::Vector3f(msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z); acc_ = Eigen::Vector3f(0, 0, 0); jrk_ = Eigen::Vector3f(0, 0, 0); - yaw_ = tf::getYaw(msg->pose.pose.orientation); + yaw_ = tf2::getYaw(msg->pose.pose.orientation); yaw_dot_ = msg->twist.twist.angular.z; // TODO: Should double check which // frame (body or world) this is in } diff --git a/trackers/kr_trackers/src/line_tracker_distance_server.cpp b/trackers/kr_trackers/src/line_tracker_distance_server.cpp index a2bd4c5d..ec0a6e30 100644 --- a/trackers/kr_trackers/src/line_tracker_distance_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_distance_server.cpp @@ -1,11 +1,12 @@ -#include -#include -#include -#include -#include -#include -#include -#include +#include "rclcpp_action/rclcpp_action.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_tracker_msgs/action/line_tracker.hpp" +#include "kr_tracker_msgs/msg/tracker_status.hpp" +#include "kr_trackers/initial_conditions.hpp" +#include "kr_trackers/Tracker.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include #include @@ -13,28 +14,34 @@ class LineTrackerDistance : public kr_trackers_manager::Tracker { public: - LineTrackerDistance(void); + explicit LineTrackerDistance(); - void Initialize(const ros::NodeHandle &nh); - bool Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); + void Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent); + bool Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd); void Deactivate(void); - kr_mav_msgs::PositionCommand::ConstPtr update(const nav_msgs::Odometry::ConstPtr &msg); + kr_mav_msgs::msg::PositionCommand::ConstSharedPtr update(const nav_msgs::msg::Odometry::SharedPtr msg); - uint8_t status() const; + uint8_t status(); - private: - void goal_callback(); + protected: + rclcpp::Logger logger_{rclcpp::get_logger("trackers_manager")}; + rclcpp::Clock::SharedPtr clock_; - void preempt_callback(); + using LineTracker = kr_tracker_msgs::action::LineTracker; + using LineTrackerGoalHandle = rclcpp_action::ServerGoalHandle; - typedef actionlib::SimpleActionServer ServerType; + 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); - // Action server that takes a goal. - // 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_; + rclcpp_action::Server::SharedPtr tracker_server_; + rclcpp::CallbackGroup::SharedPtr cb_group_; + std::shared_ptr current_goal_handle_; + rclcpp_action::GoalUUID preempted_goal_id_; + std::recursive_mutex mutex_; + bool preempt_requested_; bool pos_set_, goal_set_, goal_reached_; double default_v_des_, default_a_des_, epsilon_; float v_des_, a_des_; @@ -48,43 +55,65 @@ class LineTrackerDistance : public kr_trackers_manager::Tracker float current_traj_duration_; // Distance traveled to get to last goal. float current_traj_length_; + + std::shared_ptr result_; }; -LineTrackerDistance::LineTrackerDistance(void) : pos_set_(false), goal_set_(false), goal_reached_(true), active_(false) +LineTrackerDistance::LineTrackerDistance() + : preempt_requested_(false), + pos_set_(false), + goal_set_(false), + goal_reached_(true), + active_(false) { } -void LineTrackerDistance::Initialize(const ros::NodeHandle &nh) +void LineTrackerDistance::Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) { - ros::NodeHandle priv_nh(nh, "line_tracker_distance"); - - priv_nh.param("default_v_des", default_v_des_, 0.5); - priv_nh.param("default_a_des", default_a_des_, 0.5); - priv_nh.param("epsilon", epsilon_, 0.1); - - v_des_ = default_v_des_; - a_des_ = default_a_des_; - - // Set up the action server. - tracker_server_ = std::shared_ptr(new ServerType(priv_nh, "LineTracker", false)); - tracker_server_->registerGoalCallback(boost::bind(&LineTrackerDistance::goal_callback, this)); - tracker_server_->registerPreemptCallback(boost::bind(&LineTrackerDistance::preempt_callback, this)); - - tracker_server_->start(); + auto node = parent.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + node->declare_parameter("line_tracker_distance/default_v_des", 0.5); + node->declare_parameter("line_tracker_distance/default_a_des", 0.5); + node->declare_parameter("line_tracker_distance/epsilon", 0.1); + + default_v_des_ = node->get_parameter("line_tracker_distance/default_v_des").as_double(); + default_a_des_ = node->get_parameter("line_tracker_distance/default_a_des").as_double(); + epsilon_ = node->get_parameter("line_tracker_distance/epsilon").as_double(); + + v_des_ = static_cast(default_v_des_); + a_des_ = static_cast(default_a_des_); + + // Set up the action server + cb_group_ = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + // tracker_server_ = rclcpp_action::create_server + tracker_server_ = rclcpp_action::create_server( + node, + "~/line_tracker_distance/LineTracker", + std::bind(&LineTrackerDistance::goal_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&LineTrackerDistance::cancel_callback, this, std::placeholders::_1), + std::bind(&LineTrackerDistance::handle_accepted_callback, this, std::placeholders::_1), + rcl_action_server_get_default_options(), + cb_group_ + ); + + RCLCPP_INFO(logger_, "Initialized Line Tracker Distance"); } -bool LineTrackerDistance::Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) -{ - // Only allow activation if a goal has been set +bool LineTrackerDistance::Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) +{ + (void)cmd; if(goal_set_ && pos_set_) { - // Check that the action server has a goal. - if(!tracker_server_->isActive()) { - ROS_WARN("LineTrackerDistance::Activate: goal_set_ is true but action server has no active goal - not " - "activating."); - active_ = false; - return active_; + std::lock_guard lock(mutex_); + if(!current_goal_handle_ || !current_goal_handle_->is_active()) + { + RCLCPP_WARN(logger_, "LineTrackerDistance::Activate: goal_set_ is true but action server has no active goal - not activating."); + active_ = false; + return active_; + } } // Set start and start_yaw here so that even if the goal was sent at a @@ -102,10 +131,14 @@ bool LineTrackerDistance::Activate(const kr_mav_msgs::PositionCommand::ConstPtr void LineTrackerDistance::Deactivate(void) { - if(tracker_server_->isActive()) { - ROS_WARN("LineTrackerDistance::Deactivate: deactivated tracker while still tracking the goal."); - tracker_server_->setAborted(); + std::lock_guard lock(mutex_); + if(current_goal_handle_ && current_goal_handle_->is_active()) + { + RCLCPP_WARN(logger_, "LineTrackerDistance::Deactivate: deactivated tracker while still tracking the goal."); + current_goal_handle_->abort(result_); + current_goal_handle_.reset(); + } } ICs_.reset(); @@ -113,8 +146,10 @@ void LineTrackerDistance::Deactivate(void) active_ = false; } -kr_mav_msgs::PositionCommand::ConstPtr LineTrackerDistance::update(const nav_msgs::Odometry::ConstPtr &msg) +kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(const nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(mutex_); + // Record distance between last position and current. const float dx = Eigen::Vector3f((pos_(0) - msg->pose.pose.position.x), (pos_(1) - msg->pose.pose.position.y), (pos_(2) - msg->pose.pose.position.z)) @@ -123,33 +158,53 @@ kr_mav_msgs::PositionCommand::ConstPtr LineTrackerDistance::update(const nav_msg pos_(0) = msg->pose.pose.position.x; pos_(1) = msg->pose.pose.position.y; pos_(2) = msg->pose.pose.position.z; - yaw_ = tf::getYaw(msg->pose.pose.orientation); + yaw_ = tf2::getYaw(msg->pose.pose.orientation); pos_set_ = true; ICs_.set_from_odom(msg); - static ros::Time t_prev = msg->header.stamp; - const double dT = (msg->header.stamp - t_prev).toSec(); + static rclcpp::Time t_prev = msg->header.stamp; + const double dT = ((rclcpp::Time)msg->header.stamp - t_prev).seconds(); t_prev = msg->header.stamp; if(!active_) { - return kr_mav_msgs::PositionCommand::Ptr(); + return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); } // Track the distance and time in the current trajectory. current_traj_duration_ += dT; current_traj_length_ += dx; - kr_mav_msgs::PositionCommand::Ptr cmd(new kr_mav_msgs::PositionCommand); - cmd->header.stamp = ros::Time::now(); + auto result = std::make_shared(); + result->x = pos_(0); + result->y = pos_(1); + result->z = pos_(2); + result->yaw = yaw_; + result->duration = current_traj_duration_; + result->length = current_traj_length_; + result_ = result; + + if(current_goal_handle_ && current_goal_handle_->is_canceling()) + { + RCLCPP_INFO(logger_, "LineTrackerDistance going to goal (%2.2f, %2.2f, %2.2f) canceled.", goal_(0), goal_(1), goal_(2)); + + current_goal_handle_->canceled(result); + goal_ = pos_; + goal_set_ = false; + goal_reached_ = false; + return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); + } + + auto cmd = std::make_shared(); + cmd->header.stamp = clock_->now(); cmd->header.frame_id = msg->header.frame_id; cmd->yaw = start_yaw_; if(goal_reached_) { - if(tracker_server_->isActive()) + if(current_goal_handle_->is_active()) { - ROS_ERROR("LineTrackerDistance::update: Action server not completed.\n"); + RCLCPP_ERROR(logger_, "LineTrackerDistance::update: Action server not completed.\n"); } cmd->position.x = goal_(0), cmd->position.y = goal_(1), cmd->position.z = goal_(2); @@ -160,7 +215,7 @@ kr_mav_msgs::PositionCommand::ConstPtr LineTrackerDistance::update(const nav_msg return cmd; } - const Eigen::Vector3f dir = (goal_ - start_).normalized(); + const Eigen::Vector3f dir = (goal_ - start_).normalized(); const float total_dist = (goal_ - start_).norm(); const float d = (pos_ - start_).dot(dir); const Eigen::Vector3f proj = start_ + d * dir; @@ -170,22 +225,23 @@ kr_mav_msgs::PositionCommand::ConstPtr LineTrackerDistance::update(const nav_msg Eigen::Vector3f x(pos_), v(Eigen::Vector3f::Zero()), a(Eigen::Vector3f::Zero()); - if((pos_ - goal_).norm() <= epsilon_) // Reached goal + if((pos_ - goal_).norm() <= epsilon_) // Reached goal { - // Send a success message and reset the length and duration variables. - kr_tracker_msgs::LineTrackerResult result; - result.duration = current_traj_duration_; - result.length = current_traj_length_; - result.x = goal_(0); - result.y = goal_(1); - result.z = goal_(2); - result.yaw = start_yaw_; - tracker_server_->setSucceeded(result); + // Send success message and reset the length and duration variables + result->duration = current_traj_duration_; + result->length = current_traj_length_; + result->x = goal_(0); + result->y = goal_(1); + result->z = goal_(2); + result->yaw = start_yaw_; + current_goal_handle_->succeed(result); + result_ = result; + current_goal_handle_.reset(); current_traj_duration_ = 0.0; current_traj_length_ = 0.0; - ROS_DEBUG_THROTTLE(1, "Reached goal"); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Reached goal"); a = Eigen::Vector3f::Zero(); v = Eigen::Vector3f::Zero(); x = goal_; @@ -193,35 +249,35 @@ kr_mav_msgs::PositionCommand::ConstPtr LineTrackerDistance::update(const nav_msg } else if(d > total_dist) // Overshoot { - ROS_DEBUG_THROTTLE(1, "Overshoot"); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Overshoot"); a = -a_des_ * dir; v = Eigen::Vector3f::Zero(); x = goal_; } - else if(d >= (total_dist - ramp_dist) && d <= total_dist) // Decelerate + else if(d >= (total_dist - ramp_dist) && d <= total_dist) // Decelerate { - ROS_DEBUG_THROTTLE(1, "Decelerate"); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Decelerate"); a = -a_des_ * dir; v = std::sqrt(2 * a_des_ * (total_dist - d)) * dir; x = proj + v * dT + 0.5 * a * dT * dT; } - else if(d > ramp_dist && d < total_dist - ramp_dist) // Constant velocity + else if(d > ramp_dist && d < total_dist - ramp_dist) // Constant velocity { - ROS_DEBUG_THROTTLE(1, "Constant velocity"); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Constant velocity"); a = Eigen::Vector3f::Zero(); v = v_max * dir; x = proj + v * dT; } - else if(d >= 0 && d <= ramp_dist) // Accelerate + else if(d >= 0 && d <= ramp_dist) // Accelerate { - ROS_DEBUG_THROTTLE(1, "Accelerate"); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Accelerate"); a = a_des_ * dir; v = std::sqrt(2 * a_des_ * d) * dir; x = proj + v * dT + 0.5 * a * dT * dT; } else if(d < 0) { - ROS_DEBUG_THROTTLE(1, "Undershoot"); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Undershoot"); a = a_des_ * dir; v = Eigen::Vector3f::Zero(); x = start_ + 0.5 * a * dT * dT; @@ -233,53 +289,79 @@ kr_mav_msgs::PositionCommand::ConstPtr LineTrackerDistance::update(const nav_msg if(!goal_reached_) { - kr_tracker_msgs::LineTrackerFeedback feedback; - feedback.distance_from_goal = (pos_ - goal_).norm(); - tracker_server_->publishFeedback(feedback); + auto feedback = std::make_shared(); + feedback->distance_from_goal = (pos_ - goal_).norm(); + current_goal_handle_->publish_feedback(feedback); } return cmd; } -void LineTrackerDistance::goal_callback() +rclcpp_action::GoalResponse LineTrackerDistance::goal_callback(const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) { + (void)uuid; + (void)goal; // If another goal is already active, cancel that goal - // and track this one instead. - if(tracker_server_->isActive()) + // and track this one instead + std::lock_guard lock(mutex_); + if(current_goal_handle_ && current_goal_handle_->is_active()) { - ROS_INFO("LineTrackerDistance goal (%2.2f, %2.2f, %2.2f) aborted.", goal_(0), goal_(1), goal_(2)); - tracker_server_->setAborted(); + RCLCPP_INFO(logger_, "LineTrackerDistance goal (%2.2f, %2.2f, %2.2f) preempted.", goal_(0), goal_(1), goal_(2)); + preempted_goal_id_ = current_goal_handle_->get_goal_id(); + preempt_requested_ = true; + + goal_ = pos_; + goal_set_ = false; + goal_reached_ = false; } - // Pointer to the goal recieved. - const auto msg = tracker_server_->acceptNewGoal(); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} - current_traj_duration_ = 0.0; - current_traj_length_ = 0.0; +rclcpp_action::CancelResponse LineTrackerDistance::cancel_callback(const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(logger_, "Received Cancel Request."); + (void)goal_handle; - // If preempt has been requested, then set this goal to preempted - // and make no changes to the tracker state. - if(tracker_server_->isPreemptRequested()) + goal_ = pos_; + goal_set_ = false; + goal_reached_ = false; + + return rclcpp_action::CancelResponse::ACCEPT; +} + +void LineTrackerDistance::handle_accepted_callback(const std::shared_ptr goal_handle) +{ + std::lock_guard lock(mutex_); + + if(current_goal_handle_ && preempt_requested_) { - ROS_INFO("LineTrackerDistance going to goal (%2.2f, %2.2f, %2.2f) preempted.", msg->x, msg->y, msg->z); - tracker_server_->setPreempted(); - return; + if(current_goal_handle_->get_goal_id() == preempted_goal_id_) + { + RCLCPP_INFO(logger_, "LineTrackerDistance going to goal (%2.2f, %2.2f, %2.2f) aborted.", goal_(0), goal_(1), goal_(2)); + current_goal_handle_->abort(result_); + preempt_requested_ = false; + } } - goal_(0) = msg->x; - goal_(1) = msg->y; - goal_(2) = msg->z; + // Pointer to the goal received + current_goal_handle_ = goal_handle; + + auto goal = goal_handle->get_goal(); + goal_(0) = goal->x; + goal_(1) = goal->y; + goal_(2) = goal->z; - if(msg->relative) + if(goal->relative) goal_ += ICs_.pos(); - if(msg->v_des > 0) - v_des_ = msg->v_des; + if(goal->v_des > 0) + v_des_ = goal->v_des; else v_des_ = default_v_des_; - if(msg->a_des > 0) - a_des_ = msg->a_des; + if(goal->a_des > 0) + a_des_ = goal->a_des; else a_des_ = default_a_des_; @@ -293,31 +375,12 @@ void LineTrackerDistance::goal_callback() goal_reached_ = false; } -void LineTrackerDistance::preempt_callback() +uint8_t LineTrackerDistance::status() { - if(tracker_server_->isActive()) - { - ROS_INFO("LineTrackerDistance going to goal (%2.2f, %2.2f, %2.2f) aborted.", goal_(0), goal_(1), goal_(2)); - tracker_server_->setAborted(); - } - else - { - ROS_INFO("LineTrackerDistance going to goal (%2.2f, %2.2f, %2.2f) preempted.", goal_(0), goal_(1), goal_(2)); - tracker_server_->setPreempted(); - } - - // TODO: How much overshoot will this cause at high velocities? - goal_ = pos_; + std::lock_guard lock(mutex_); - goal_set_ = false; - goal_reached_ = true; -} - -uint8_t LineTrackerDistance::status() const -{ - return tracker_server_->isActive() ? static_cast(kr_tracker_msgs::TrackerStatus::ACTIVE) : - static_cast(kr_tracker_msgs::TrackerStatus::SUCCEEDED); + return current_goal_handle_->is_active() ? static_cast(kr_tracker_msgs::msg::TrackerStatus::ACTIVE) : static_cast(kr_tracker_msgs::msg::TrackerStatus::SUCCEEDED); } -#include -PLUGINLIB_EXPORT_CLASS(LineTrackerDistance, kr_trackers_manager::Tracker); +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(LineTrackerDistance, kr_trackers_manager::Tracker) 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 669ac35f..5aed4ad2 100644 --- a/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp @@ -1,11 +1,12 @@ -#include -#include -#include -#include -#include -#include -#include -#include +#include "rclcpp_action/rclcpp_action.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_tracker_msgs/action/line_tracker.hpp" +#include "kr_tracker_msgs/msg/tracker_status.hpp" +#include "kr_trackers/initial_conditions.hpp" +#include "kr_trackers/Tracker.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include #include @@ -15,30 +16,35 @@ class LineTrackerMinJerk : public kr_trackers_manager::Tracker public: LineTrackerMinJerk(void); - void Initialize(const ros::NodeHandle &nh); - bool Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); + void Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent); + bool Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd); void Deactivate(void); - kr_mav_msgs::PositionCommand::ConstPtr update(const nav_msgs::Odometry::ConstPtr &msg); + kr_mav_msgs::msg::PositionCommand::ConstSharedPtr update(const nav_msgs::msg::Odometry::SharedPtr msg); - uint8_t status() const; + uint8_t status(); - private: - void goal_callback(); + protected: + rclcpp::Logger logger_{rclcpp::get_logger("trackers_manager")}; + rclcpp::Clock::SharedPtr clock_; - void preempt_callback(); + using LineTracker = kr_tracker_msgs::action::LineTracker; + using LineTrackerGoalHandle = 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); void gen_trajectory(const Eigen::Vector3f &xi, const Eigen::Vector3f &xf, const Eigen::Vector3f &vi, const Eigen::Vector3f &vf, const Eigen::Vector3f &ai, const Eigen::Vector3f &af, const float &yawi, const float &yawf, const float &yaw_dot_i, const float &yaw_dot_f, float dt, Eigen::Vector3f coeffs[6], float yaw_coeffs[4]); - typedef actionlib::SimpleActionServer ServerType; - - // Action server that takes a goal. - // 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_; + rclcpp_action::Server::SharedPtr tracker_server_; + rclcpp::CallbackGroup::SharedPtr cb_group_; + std::shared_ptr current_goal_handle_; + rclcpp_action::GoalUUID preempted_goal_id_; + std::recursive_mutex mutex_; Eigen::Vector3f current_pos_; @@ -46,76 +52,100 @@ class LineTrackerMinJerk : public kr_trackers_manager::Tracker double default_v_des_, default_a_des_, default_yaw_v_des_, default_yaw_a_des_; float v_des_, a_des_, yaw_v_des_, yaw_a_des_; bool active_; + bool preempt_requested_; InitialConditions ICs_; Eigen::Vector3f goal_; - ros::Time traj_start_; + bool traj_start_set_; + rclcpp::Time traj_start_; float traj_duration_; - ros::Duration goal_duration_; + rclcpp::Duration goal_duration_ = rclcpp::Duration(0, 0); Eigen::Vector3f coeffs_[6]; float goal_yaw_, yaw_coeffs_[4]; - bool traj_start_set_; float current_traj_length_; + std::shared_ptr result_; }; LineTrackerMinJerk::LineTrackerMinJerk(void) - : pos_set_(false), - goal_set_(false), - goal_reached_(true), - active_(false), - traj_start_(ros::Time::now()), - traj_start_set_(false) + : pos_set_(false), + goal_set_(false), + goal_reached_(true), + active_(false), + preempt_requested_(false), + traj_start_set_(false) { } -void LineTrackerMinJerk::Initialize(const ros::NodeHandle &nh) +void LineTrackerMinJerk::Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) { - ros::NodeHandle priv_nh(nh, "line_tracker_min_jerk"); + auto node = parent.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); - priv_nh.param("default_v_des", default_v_des_, 0.5); - priv_nh.param("default_a_des", default_a_des_, 0.3); - priv_nh.param("default_yaw_v_des", default_yaw_v_des_, 0.8); - priv_nh.param("default_yaw_a_des", default_yaw_a_des_, 0.2); + node->declare_parameter("line_tracker_min_jerk/default_v_des", 0.5); + node->declare_parameter("line_tracker_min_jerk/default_a_des", 0.3); + node->declare_parameter("line_tracker_min_jerk/default_yaw_v_des", 0.8); + node->declare_parameter("line_tracker_min_jerk/default_yaw_a_des", 0.2); + + default_v_des_ = node->get_parameter("line_tracker_min_jerk/default_v_des").as_double(); + default_a_des_ = node->get_parameter("line_tracker_min_jerk/default_a_des").as_double(); + yaw_v_des_ = node->get_parameter("line_tracker_min_jerk/default_yaw_v_des").as_double(); + yaw_a_des_ = node->get_parameter("line_tracker_min_jerk/default_yaw_a_des").as_double(); v_des_ = default_v_des_; a_des_ = default_a_des_; yaw_v_des_ = default_yaw_v_des_; yaw_a_des_ = default_yaw_a_des_; + + traj_start_ = clock_->now(); // Set up the action server. - tracker_server_ = std::shared_ptr(new ServerType(priv_nh, "LineTracker", false)); - tracker_server_->registerGoalCallback(boost::bind(&LineTrackerMinJerk::goal_callback, this)); - tracker_server_->registerPreemptCallback(boost::bind(&LineTrackerMinJerk::preempt_callback, this)); - - tracker_server_->start(); + cb_group_ = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + tracker_server_ = rclcpp_action::create_server( + node, + "~/line_tracker_min_jerk/LineTracker", + std::bind(&LineTrackerMinJerk::goal_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&LineTrackerMinJerk::cancel_callback, this, std::placeholders::_1), + std::bind(&LineTrackerMinJerk::handle_accepted_callback, this, std::placeholders::_1), + rcl_action_server_get_default_options(), + cb_group_ + ); + + RCLCPP_INFO(logger_, "Initialized Line Tracker Min Jerk"); } -bool LineTrackerMinJerk::Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) +bool LineTrackerMinJerk::Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) { + (void)cmd; + // Only allow activation if a goal has been set if(goal_set_ && pos_set_) { - if(!tracker_server_->isActive()) { - ROS_WARN("LineTrackerMinJerk::Activate: goal_set_ is true but action server has no active goal - not " - "activating."); - active_ = false; - return false; - } - active_ = true; + std::lock_guard lock_(mutex_); + if(!current_goal_handle_ || !current_goal_handle_->is_active()) + { + RCLCPP_WARN(logger_, "LineTrackerMinJerk::Activate: goal_set_ is true but action server has no active goal - not activating."); + active_ = false; + return active_; + } + active_ = true; - current_traj_length_ = 0.0; + current_traj_length_ = 0.0; + } } return active_; } void LineTrackerMinJerk::Deactivate(void) { - if(tracker_server_->isActive()) + std::lock_guard lock(mutex_); + if(current_goal_handle_ && current_goal_handle_->is_active()) { - ROS_WARN("LineTrackerMinJerk::Deactivate: deactivated tracker while still tracking the goal."); - tracker_server_->setAborted(); + RCLCPP_WARN(logger_, "LineTrackerMinJerk::Deactivate: deactivated tracker while still tracking the goal."); + current_goal_handle_->abort(result_); + current_goal_handle_.reset(); } ICs_.reset(); @@ -123,8 +153,10 @@ void LineTrackerMinJerk::Deactivate(void) active_ = false; } -kr_mav_msgs::PositionCommand::ConstPtr LineTrackerMinJerk::update(const nav_msgs::Odometry::ConstPtr &msg) +kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerMinJerk::update(const nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(mutex_); + // Record distance between last position and current. const float dx = Eigen::Vector3f((current_pos_(0) - msg->pose.pose.position.x), (current_pos_(1) - msg->pose.pose.position.y), @@ -138,16 +170,36 @@ kr_mav_msgs::PositionCommand::ConstPtr LineTrackerMinJerk::update(const nav_msgs pos_set_ = true; ICs_.set_from_odom(msg); - const ros::Time t_now = ros::Time::now(); - + const rclcpp::Time t_now = clock_->now(); + if(!active_) { - return kr_mav_msgs::PositionCommand::Ptr(); + return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); } current_traj_length_ += dx; - kr_mav_msgs::PositionCommand::Ptr cmd(new kr_mav_msgs::PositionCommand); + auto result = std::make_shared(); + result->x = current_pos_(0); + result->y = current_pos_(1); + result->z = current_pos_(2); + result->yaw = ICs_.yaw(); + result->length = current_traj_length_; + result->duration = (t_now - traj_start_).seconds(); + result_ = result; + + if(current_goal_handle_ && current_goal_handle_->is_canceling()) + { + RCLCPP_INFO(logger_, "LineTrackerDistance going to goal (%2.2f, %2.2f, %2.2f) canceled.", goal_(0), goal_(1), goal_(2)); + + current_goal_handle_->canceled(result); + goal_ = current_pos_; + goal_set_ = false; + goal_reached_ = false; + return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); + } + + auto cmd = std::make_shared(); cmd->header.stamp = t_now; cmd->header.frame_id = msg->header.frame_id; @@ -160,9 +212,9 @@ kr_mav_msgs::PositionCommand::ConstPtr LineTrackerMinJerk::update(const nav_msgs traj_duration_ = 0.5f; // TODO: This should probably be after traj_duration_ is determined - if(goal_duration_.toSec() > traj_duration_) + if(goal_duration_.seconds() > traj_duration_) { - traj_duration_ = goal_duration_.toSec(); + traj_duration_ = goal_duration_.seconds(); duration_set = true; } @@ -250,9 +302,9 @@ kr_mav_msgs::PositionCommand::ConstPtr LineTrackerMinJerk::update(const nav_msgs } else if(goal_reached_) { - if(tracker_server_->isActive()) + if(current_goal_handle_->is_active()) { - ROS_ERROR("LineTrackerDistance::update: Action server not completed.\n"); + RCLCPP_ERROR(logger_, "LineTrackerMinJerk::update: Action server not completed.\n"); } cmd->position.x = goal_(0), cmd->position.y = goal_(1), cmd->position.z = goal_(2); @@ -268,24 +320,23 @@ kr_mav_msgs::PositionCommand::ConstPtr LineTrackerMinJerk::update(const nav_msgs Eigen::Vector3f x(ICs_.pos()), v(Eigen::Vector3f::Zero()), a(Eigen::Vector3f::Zero()), j(Eigen::Vector3f::Zero()); float yaw_des(ICs_.yaw()), yaw_dot_des(0); - const float traj_time = (t_now - traj_start_).toSec(); + const float traj_time = (t_now - traj_start_).seconds(); if(traj_time >= traj_duration_) // Reached goal { // Send a success message and reset the length and duration variables. - kr_tracker_msgs::LineTrackerResult result; - result.duration = traj_time; - result.length = current_traj_length_; - result.x = goal_(0); - result.y = goal_(1); - result.z = goal_(2); - result.yaw = goal_yaw_; - - tracker_server_->setSucceeded(result); - + result->duration = traj_time; + result->length = current_traj_length_; + result->x = goal_(0); + result->y = goal_(1); + result->z = goal_(2); + result->yaw = goal_yaw_; + + current_goal_handle_->succeed(result); + result_ = result; current_traj_length_ = 0.0; - ROS_DEBUG_THROTTLE(1, "Reached goal"); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Reached goal"); j = Eigen::Vector3f::Zero(); a = Eigen::Vector3f::Zero(); v = Eigen::Vector3f::Zero(); @@ -313,7 +364,7 @@ kr_mav_msgs::PositionCommand::ConstPtr LineTrackerMinJerk::update(const nav_msgs yaw_dot_des = yaw_dot_des / traj_duration_; } else // (traj_time < 0) can happen when t_start is set - ROS_INFO_THROTTLE(1, "Trajectory hasn't started yet"); + RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, "Trajectory hasn't started yet"); cmd->position.x = x(0), cmd->position.y = x(1), cmd->position.z = x(2); cmd->yaw = yaw_des; @@ -326,44 +377,72 @@ kr_mav_msgs::PositionCommand::ConstPtr LineTrackerMinJerk::update(const nav_msgs if(!goal_reached_) { - kr_tracker_msgs::LineTrackerFeedback feedback; - feedback.distance_from_goal = (current_pos_ - goal_).norm(); - tracker_server_->publishFeedback(feedback); + auto feedback = std::make_shared(); + feedback->distance_from_goal = (current_pos_ - goal_).norm(); + current_goal_handle_->publish_feedback(feedback); } return cmd; } -void LineTrackerMinJerk::goal_callback() +rclcpp_action::GoalResponse LineTrackerMinJerk::goal_callback(const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) { + (void)uuid; + (void)goal; + // If another goal is already active, cancel that goal // and track this one instead. - if(tracker_server_->isActive()) + std::lock_guard lock(mutex_); + if(current_goal_handle_ && current_goal_handle_->is_active()) { - ROS_INFO("LineTrackerMinJerk goal (%f, %f, %f) aborted.", goal_(0), goal_(1), goal_(2)); - tracker_server_->setAborted(); + RCLCPP_INFO(logger_, "LineTrackerMinJerk goal (%f, %f, %f) preempted.", goal_(0), goal_(1), goal_(2)); + preempted_goal_id_ = current_goal_handle_->get_goal_id(); + preempt_requested_ = true; + + goal_ = current_pos_; + goal_set_ = false; + goal_reached_ = false; } - // Pointer to the goal recieved. - const auto msg = tracker_server_->acceptNewGoal(); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse LineTrackerMinJerk::cancel_callback(const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(logger_, "Received Cancel Request."); + (void)goal_handle; - current_traj_length_ = 0.0; + goal_ = ICs_.pos(); + goal_set_ = false; + goal_reached_ = true; + + return rclcpp_action::CancelResponse::ACCEPT; +} - // If preempt has been requested, then set this goal to preempted - // and make no changes to the tracker state. - if(tracker_server_->isPreemptRequested()) +void LineTrackerMinJerk::handle_accepted_callback(const std::shared_ptr goal_handle) +{ + std::lock_guard lock(mutex_); + + if(current_goal_handle_ && preempt_requested_) { - ROS_INFO("LineTrackerMinJerk going to goal (%f, %f, %f, %f) preempted.", msg->x, msg->y, msg->z, msg->yaw); - tracker_server_->setPreempted(); - return; + if(current_goal_handle_->get_goal_id() == preempted_goal_id_) + { + RCLCPP_INFO(logger_, "LineTrackerMinJerk going to goal (%2.2f, %2.2f, %2.2f) aborted.", goal_(0), goal_(1), goal_(2)); + current_goal_handle_->abort(result_); + preempt_requested_ = false; + } } + // Pointer to the goal received + current_goal_handle_ = goal_handle; + + auto msg = goal_handle->get_goal(); goal_(0) = msg->x; goal_(1) = msg->y; goal_(2) = msg->z; goal_yaw_ = msg->yaw; goal_duration_ = msg->duration; - if(msg->t_start != ros::Time(0)) + if(msg->t_start != rclcpp::Time()) { traj_start_ = msg->t_start; traj_start_set_ = true; @@ -375,38 +454,18 @@ void LineTrackerMinJerk::goal_callback() { goal_ += ICs_.pos(); goal_yaw_ += ICs_.yaw(); - ROS_INFO("line_tracker_min_jerk using relative command"); + RCLCPP_INFO(logger_, "line_tracker_min_jerk using relative command"); } v_des_ = (msg->v_des > 0.0) ? msg->v_des : default_v_des_; a_des_ = (msg->a_des > 0.0) ? msg->a_des : default_a_des_; - ROS_DEBUG("line_tracker_min_jerk using v_des = %2.2f m/s and a_des = %2.2f m/s^2", v_des_, a_des_); + RCLCPP_DEBUG(logger_, "line_tracker_min_jerk using v_des = %2.2f m/s and a_des = %2.2f m/s^2", v_des_, a_des_); goal_set_ = true; goal_reached_ = false; } -void LineTrackerMinJerk::preempt_callback() -{ - if(tracker_server_->isActive()) - { - ROS_INFO("LineTrackerMinJerk going to goal (%f, %f, %f) aborted.", goal_(0), goal_(1), goal_(2)); - tracker_server_->setAborted(); - } - else - { - ROS_INFO("LineTrackerMinJerk going to goal (%f, %f, %f) preempted.", goal_(0), goal_(1), goal_(2)); - tracker_server_->setPreempted(); - } - - // TODO: How much overshoot will this cause at high velocities? - goal_ = ICs_.pos(); - - goal_set_ = false; - goal_reached_ = true; -} - void LineTrackerMinJerk::gen_trajectory(const Eigen::Vector3f &xi, const Eigen::Vector3f &xf, const Eigen::Vector3f &vi, const Eigen::Vector3f &vf, const Eigen::Vector3f &ai, const Eigen::Vector3f &af, const float &yawi, const float &yawf, const float &yaw_dot_i, @@ -446,11 +505,12 @@ void LineTrackerMinJerk::gen_trajectory(const Eigen::Vector3f &xi, const Eigen:: } } -uint8_t LineTrackerMinJerk::status() const +uint8_t LineTrackerMinJerk::status() { - return tracker_server_->isActive() ? static_cast(kr_tracker_msgs::TrackerStatus::ACTIVE) : - static_cast(kr_tracker_msgs::TrackerStatus::SUCCEEDED); + std::lock_guard lock(mutex_); + + return current_goal_handle_->is_active() ? static_cast(kr_tracker_msgs::msg::TrackerStatus::ACTIVE) : static_cast(kr_tracker_msgs::msg::TrackerStatus::SUCCEEDED); } -#include +#include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(LineTrackerMinJerk, kr_trackers_manager::Tracker); diff --git a/trackers/kr_trackers/src/line_tracker_trapezoid.cpp b/trackers/kr_trackers/src/line_tracker_trapezoid.cpp index 10846510..8031d2c8 100644 --- a/trackers/kr_trackers/src/line_tracker_trapezoid.cpp +++ b/trackers/kr_trackers/src/line_tracker_trapezoid.cpp @@ -1,3 +1,5 @@ +// TODO: convert to ros2 compatible format + // TODO: make into actionlib #include diff --git a/trackers/kr_trackers/src/line_tracker_yaw.cpp b/trackers/kr_trackers/src/line_tracker_yaw.cpp index b2e6931f..8559cf3a 100644 --- a/trackers/kr_trackers/src/line_tracker_yaw.cpp +++ b/trackers/kr_trackers/src/line_tracker_yaw.cpp @@ -1,3 +1,5 @@ +// TODO: convert to ros2 compatible format + // TODO: make into actionlib #include diff --git a/trackers/kr_trackers/src/lissajous_adder_server.cpp b/trackers/kr_trackers/src/lissajous_adder_server.cpp index 89060dde..45546008 100644 --- a/trackers/kr_trackers/src/lissajous_adder_server.cpp +++ b/trackers/kr_trackers/src/lissajous_adder_server.cpp @@ -1,3 +1,5 @@ +// TODO: convert to ros2 compatible format + #include #include #include diff --git a/trackers/kr_trackers/src/lissajous_generator.cpp b/trackers/kr_trackers/src/lissajous_generator.cpp index 070721af..f272c7df 100644 --- a/trackers/kr_trackers/src/lissajous_generator.cpp +++ b/trackers/kr_trackers/src/lissajous_generator.cpp @@ -1,3 +1,5 @@ +// TODO: convert to ros2 compatible format + #include #include diff --git a/trackers/kr_trackers/src/lissajous_tracker_server.cpp b/trackers/kr_trackers/src/lissajous_tracker_server.cpp index 0c3f0c90..69da3c4c 100644 --- a/trackers/kr_trackers/src/lissajous_tracker_server.cpp +++ b/trackers/kr_trackers/src/lissajous_tracker_server.cpp @@ -1,3 +1,5 @@ +// TODO: convert to ros2 compatible format + #include #include #include diff --git a/trackers/kr_trackers/src/null_tracker.cpp b/trackers/kr_trackers/src/null_tracker.cpp index 23588486..4ab72b41 100644 --- a/trackers/kr_trackers/src/null_tracker.cpp +++ b/trackers/kr_trackers/src/null_tracker.cpp @@ -1,3 +1,5 @@ +// TODO: convert to ros2 compatible format + #include #include #include diff --git a/trackers/kr_trackers/src/smooth_vel_tracker_server.cpp b/trackers/kr_trackers/src/smooth_vel_tracker_server.cpp index 0ce720e5..73d17609 100644 --- a/trackers/kr_trackers/src/smooth_vel_tracker_server.cpp +++ b/trackers/kr_trackers/src/smooth_vel_tracker_server.cpp @@ -1,3 +1,5 @@ +// TODO: convert to ros2 compatible format + #include #include #include diff --git a/trackers/kr_trackers/src/trajectory_tracker.cpp b/trackers/kr_trackers/src/trajectory_tracker.cpp index 8b9bafcb..3109bf27 100644 --- a/trackers/kr_trackers/src/trajectory_tracker.cpp +++ b/trackers/kr_trackers/src/trajectory_tracker.cpp @@ -1,3 +1,5 @@ +// TODO: convert to ros2 compatible format + #include #include diff --git a/trackers/kr_trackers/src/velocity_tracker.cpp b/trackers/kr_trackers/src/velocity_tracker.cpp index 914bd12a..3a2f8471 100644 --- a/trackers/kr_trackers/src/velocity_tracker.cpp +++ b/trackers/kr_trackers/src/velocity_tracker.cpp @@ -1,3 +1,5 @@ +// TODO: convert to ros2 compatible format + #include #include #include From 460e403cf3a02d912a8db3aed39789b059db8b6c Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Sat, 5 Oct 2024 11:10:49 -0500 Subject: [PATCH 16/64] [WIP]: kr_trackers_manager ros2 compatible --- trackers/kr_trackers/package.xml | 1 + trackers/kr_trackers_manager/CMakeLists.txt | 84 +++++--- trackers/kr_trackers_manager/README.md | 15 ++ .../config/trackers_manager.yaml | 10 + .../include/kr_trackers_manager/Tracker.h | 59 ------ .../launch/example.launch.py | 43 ++++ .../kr_trackers_manager/nodelet_plugin.xml | 6 - trackers/kr_trackers_manager/package.xml | 24 ++- .../src/lifecycle_manager.cpp | 71 +++++++ .../src/trackers_manager.cpp | 196 ++++++++++-------- 10 files changed, 315 insertions(+), 194 deletions(-) create mode 100644 trackers/kr_trackers_manager/README.md create mode 100644 trackers/kr_trackers_manager/config/trackers_manager.yaml delete mode 100644 trackers/kr_trackers_manager/include/kr_trackers_manager/Tracker.h create mode 100644 trackers/kr_trackers_manager/launch/example.launch.py delete mode 100644 trackers/kr_trackers_manager/nodelet_plugin.xml create mode 100644 trackers/kr_trackers_manager/src/lifecycle_manager.cpp diff --git a/trackers/kr_trackers/package.xml b/trackers/kr_trackers/package.xml index c6ab0203..60a6794a 100644 --- a/trackers/kr_trackers/package.xml +++ b/trackers/kr_trackers/package.xml @@ -7,6 +7,7 @@ Kartik Mohta Michael Watterson Justin Thomas + Kashish Garg Pranav Shah BSD diff --git a/trackers/kr_trackers_manager/CMakeLists.txt b/trackers/kr_trackers_manager/CMakeLists.txt index 6a522c6e..cd4ab8c0 100644 --- a/trackers/kr_trackers_manager/CMakeLists.txt +++ b/trackers/kr_trackers_manager/CMakeLists.txt @@ -1,48 +1,64 @@ cmake_minimum_required(VERSION 3.10) project(kr_trackers_manager) -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) - -find_package( - catkin REQUIRED - COMPONENTS roscpp - nodelet - pluginlib - nav_msgs - kr_mav_msgs - kr_tracker_msgs) - -catkin_package( - INCLUDE_DIRS +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcutils REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(kr_mav_msgs REQUIRED) +find_package(kr_trackers REQUIRED) +find_package(kr_tracker_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(lifecycle_msgs REQUIRED) +# find_package(geometry_msgs REQUIRED) +# find_package(tf2 REQUIRED) +# find_package(tf2_geometry_msgs REQUIRED) + +include_directories( include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - roscpp - nodelet +) + +set(plugins_dependencies pluginlib +) + +set(node_dependencies + ${plugins_dependencies} + rclcpp + rclcpp_components + rclcpp_lifecycle + rcutils kr_mav_msgs + kr_trackers + kr_tracker_msgs nav_msgs - kr_tracker_msgs) +) -add_library(${PROJECT_NAME} src/trackers_manager.cpp) -target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +add_library(${PROJECT_NAME} SHARED src/trackers_manager.cpp src/lifecycle_manager.cpp) +ament_target_dependencies(${PROJECT_NAME} ${node_dependencies} lifecycle_msgs) +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +rclcpp_components_register_nodes(${PROJECT_NAME} "TrackersManager") +rclcpp_components_register_nodes(${PROJECT_NAME} "TrackersManagerLifecycleManager") install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) -install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME} +) + +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/trackers/kr_trackers_manager/README.md b/trackers/kr_trackers_manager/README.md new file mode 100644 index 00000000..b79b4ee6 --- /dev/null +++ b/trackers/kr_trackers_manager/README.md @@ -0,0 +1,15 @@ +kr_trackers_manager +============= + +Package for TrackersManager. + +## Requirements +1. Package `kr_trackers` +2. Lifecycle Node manager + +## Build Package +TODO: insert instructions + +## Run Tests +TODO: insert instructions + diff --git a/trackers/kr_trackers_manager/config/trackers_manager.yaml b/trackers/kr_trackers_manager/config/trackers_manager.yaml new file mode 100644 index 00000000..40bb0c26 --- /dev/null +++ b/trackers/kr_trackers_manager/config/trackers_manager.yaml @@ -0,0 +1,10 @@ +trackers_manager: + ros__parameters: + trackers: ["LineTrackerDistance", "LineTrackerMinJerk"] + line_tracker_distance/default_v_des: 0.5 + line_tracker_distance/default_a_des: 0.5 + line_tracker_distance/epsilon: 0.1 + line_tracker_min_jerk/default_v_des: 0.5 + 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 diff --git a/trackers/kr_trackers_manager/include/kr_trackers_manager/Tracker.h b/trackers/kr_trackers_manager/include/kr_trackers_manager/Tracker.h deleted file mode 100644 index f9748a6a..00000000 --- a/trackers/kr_trackers_manager/include/kr_trackers_manager/Tracker.h +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef TRACKERS_MANAGER_TRACKER_H_ -#define TRACKERS_MANAGER_TRACKER_H_ - -#include -#include -#include - -namespace kr_trackers_manager -{ -class Tracker -{ - public: - virtual ~Tracker(void) {} - - /** - * @brief Initialize the tracker. Should be used to get the params, construct the publishers and subscribers. - * - * @param nh The NodeHandle with the kr_trackers_manager's namespace, can be used to read common params such as gains. - */ - virtual void Initialize(const ros::NodeHandle &nh) = 0; - - /** - * @brief Activate the tracker. This indicates that the tracker should get ready to publish commands. - * - * @param cmd The last PositionCommand that was published, can be used to maintain continuity of commands when - * switching trackers. - * - * @return Should return true if the tracker is ready to publish commands, else return false. - */ - virtual bool Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) = 0; - - /** - * @brief Deactivate the tracker. This is called when the kr_trackers_manager switches to another tracker. - */ - virtual void Deactivate(void) = 0; - - /** - * @brief Get the current command output from the tracker. - * Note that this function is still called even if the tracker has not been activated. This is for cases when the - * tracker would want to use the previous robot odometry to compute current commands. - * - * @param msg The current odometry message which should be used by the tracker to generate the command. - * - * @return The PositionCommand message which would be published. If an uninitialized ConstPtr is returned, then no - * PositionCommand message would be published. - */ - virtual kr_mav_msgs::PositionCommand::ConstPtr update(const nav_msgs::Odometry::ConstPtr &msg) = 0; - - /** - * @brief Get status of the tracker. Only called when the tracker has been activated. - * - * @return The tracker status (see the options in the TrackerStatus message). - */ - virtual uint8_t status() const = 0; -}; - -} // namespace kr_trackers_manager - -#endif diff --git a/trackers/kr_trackers_manager/launch/example.launch.py b/trackers/kr_trackers_manager/launch/example.launch.py new file mode 100644 index 00000000..fdfefd2f --- /dev/null +++ b/trackers/kr_trackers_manager/launch/example.launch.py @@ -0,0 +1,43 @@ +""" +This launch file is just an example of how to launch the TrackersManager along with its lifecycle manager +""" + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + ld = LaunchDescription() + + # Path to the configuration file + config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml' + + container = ComposableNodeContainer( + name="trackers_manager_container", + namespace="", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions= [ + ComposableNode( + package="kr_trackers_manager", + plugin="TrackersManager", + name="trackers_manager", + parameters=[config_file] + ), + ComposableNode( + package="kr_trackers_manager", + plugin="TrackersManagerLifecycleManager", + name="lifecycle_manager", + parameters=[ + {'node_name': "trackers_manager"} + ] + ) + ], + output='screen' + ) + + ld.add_action(container) + + return ld diff --git a/trackers/kr_trackers_manager/nodelet_plugin.xml b/trackers/kr_trackers_manager/nodelet_plugin.xml deleted file mode 100644 index fb3dfd1a..00000000 --- a/trackers/kr_trackers_manager/nodelet_plugin.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/trackers/kr_trackers_manager/package.xml b/trackers/kr_trackers_manager/package.xml index aa5fc728..d4f1d1ed 100644 --- a/trackers/kr_trackers_manager/package.xml +++ b/trackers/kr_trackers_manager/package.xml @@ -1,25 +1,33 @@ - + + kr_trackers_manager - 1.0.0 + 2.0.0 kr_trackers_manager Kartik Mohta Michael Watterson Justin Thomas + Kashish Garg + Pranav Shah BSD Kartik Mohta - catkin - roscpp - nodelet - pluginlib + ament_cmake + + rclcpp + rcutils + rclcpp_components + rclcpp_lifecycle kr_mav_msgs - nav_msgs + kr_trackers kr_tracker_msgs + nav_msgs + pluginlib + lifecycle_msgs - + ament_cmake diff --git a/trackers/kr_trackers_manager/src/lifecycle_manager.cpp b/trackers/kr_trackers_manager/src/lifecycle_manager.cpp new file mode 100644 index 00000000..64a1ab59 --- /dev/null +++ b/trackers/kr_trackers_manager/src/lifecycle_manager.cpp @@ -0,0 +1,71 @@ +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + +using LifecycleCallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class TrackersManagerLifecycleManager : public rclcpp_lifecycle::LifecycleNode +{ + public: + TrackersManagerLifecycleManager(const rclcpp::NodeOptions &options); + ~TrackersManagerLifecycleManager(); + // LifecycleCallbackReturn on_shutdown(const rclcpp_lifecycle::State &previous_state); + + private: + rclcpp::Client::SharedPtr client_; + std::string node_name; + std::string service_change_state_name; +}; + +/* + * @brief This is a lifecycle node manager for TrackersManager LifecycleNode. + */ +TrackersManagerLifecycleManager::TrackersManagerLifecycleManager(const rclcpp::NodeOptions &options) + : LifecycleNode("lifecycle_manager", rclcpp::NodeOptions(options).use_intra_process_comms(true)) +{ + this->declare_parameter("node_name", "trackers_manager"); + node_name = this->get_parameter("node_name").as_string(); + service_change_state_name = std::string("/") + node_name + std::string("/change_state"); + client_ = this->create_client(service_change_state_name); + + client_->wait_for_service(); + + // Configure node + RCLCPP_INFO(this->get_logger(), "Trying to configure node"); + auto transition = lifecycle_msgs::msg::Transition(); + transition.id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE; + transition.label = std::string("configure"); + auto request = std::make_shared(); + request->transition = transition; + auto future = client_->async_send_request(request); + if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) + { + auto response = future.get(); + if(response->success) + RCLCPP_INFO(this->get_logger(), "Node configured successfully"); + } + + // TODO: If node not configured, then don't run activation sequence. + + // Activate node + RCLCPP_INFO(this->get_logger(), "Trying to activate node"); + transition = lifecycle_msgs::msg::Transition(); + transition.id = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE; + transition.label = std::string("activate"); + request = std::make_shared(); + request->transition = transition; + future = client_->async_send_request(request); + if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) + { + auto response = future.get(); + if(response->success) + RCLCPP_INFO(this->get_logger(), "Node activated successfully"); + } +} + +TrackersManagerLifecycleManager::~TrackersManagerLifecycleManager() +{} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(TrackersManagerLifecycleManager) diff --git a/trackers/kr_trackers_manager/src/trackers_manager.cpp b/trackers/kr_trackers_manager/src/trackers_manager.cpp index b53fad84..bf01078a 100644 --- a/trackers/kr_trackers_manager/src/trackers_manager.cpp +++ b/trackers/kr_trackers_manager/src/trackers_manager.cpp @@ -1,111 +1,127 @@ -#include -#include -#include -#include -#include -#include -#include - -class TrackersManager : public nodelet::Nodelet +#include "kr_tracker_msgs/msg/tracker_status.hpp" +#include "kr_tracker_msgs/srv/transition.hpp" +#include "kr_trackers/Tracker.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/node_factory.hpp" +#include "rclcpp_components/component_manager.hpp" +#include "composition_interfaces/srv/load_node.hpp" +#include +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using LifecycleCallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class TrackersManager : public rclcpp_lifecycle::LifecycleNode { public: - TrackersManager(void); + TrackersManager(const rclcpp::NodeOptions &options); ~TrackersManager(void); - void onInit(void); - - private: - void odom_callback(const nav_msgs::Odometry::ConstPtr &msg); - bool transition_callback(kr_tracker_msgs::Transition::Request &req, kr_tracker_msgs::Transition::Response &res); - - ros::Subscriber sub_odom_; - ros::Publisher pub_cmd_, pub_status_; - ros::ServiceServer srv_tracker_; + // std::shared_ptr ptr_; + + LifecycleCallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state); + LifecycleCallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state); + LifecycleCallbackReturn on_shutdown(const rclcpp_lifecycle::State &previous_state); + + protected: + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + void transition_callback(const kr_tracker_msgs::srv::Transition::Request::SharedPtr req, + const kr_tracker_msgs::srv::Transition::Response::SharedPtr res); + + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Service::SharedPtr srv_tracker_; pluginlib::ClassLoader tracker_loader_; - kr_trackers_manager::Tracker *active_tracker_; - std::map tracker_map_; - kr_mav_msgs::PositionCommand::ConstPtr cmd_; + // kr_trackers_manager::Tracker *active_tracker; // switched this to use smart pointers + std::shared_ptr active_tracker_; + // std::map tracker_map_; // switched this to use smart pointers + std::map> tracker_map_; + kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd_; }; -TrackersManager::TrackersManager(void) - : tracker_loader_("kr_trackers_manager", "kr_trackers_manager::Tracker"), active_tracker_(NULL) +TrackersManager::TrackersManager(const rclcpp::NodeOptions &options) + : LifecycleNode("trackers_manager", options), + tracker_loader_("kr_trackers", "kr_trackers_manager::Tracker") { + RCLCPP_INFO(this->get_logger(), "IN TRACKERS MANAGER CONSTRUCTOR"); } TrackersManager::~TrackersManager(void) { - for(std::map::iterator it = tracker_map_.begin(); - it != tracker_map_.end(); it++) - { - delete it->second; -#if ROS_VERSION_MINIMUM(1, 8, 0) - try - { - tracker_loader_.unloadLibraryForClass(it->first); - } - catch(pluginlib::LibraryUnloadException &e) - { - NODELET_ERROR_STREAM("Could not unload library for the tracker " << it->first << ": " << e.what()); - } -#endif - } } -void TrackersManager::onInit(void) +LifecycleCallbackReturn TrackersManager::on_configure(const rclcpp_lifecycle::State &previous_state) { - ros::NodeHandle priv_nh(getPrivateNodeHandle()); + (void)previous_state; + + RCLCPP_INFO(this->get_logger(), "Configuring TrackersManager and Trackers."); - XmlRpc::XmlRpcValue tracker_list; - priv_nh.getParam("trackers", tracker_list); - ROS_ASSERT(tracker_list.getType() == XmlRpc::XmlRpcValue::TypeArray); - for(int i = 0; i < tracker_list.size(); i++) + auto node = shared_from_this(); + std::weak_ptr weak_node = node; + + this->declare_parameter>("trackers", {"LineTrackerDistance"}); + std::vector trackers_array; + this->get_parameter("trackers", trackers_array); + + for(auto name: trackers_array) { - ROS_ASSERT(tracker_list[i].getType() == XmlRpc::XmlRpcValue::TypeString); - const std::string tracker_name = static_cast(tracker_list[i]); try { -#if ROS_VERSION_MINIMUM(1, 8, 0) - kr_trackers_manager::Tracker *c = tracker_loader_.createUnmanagedInstance(tracker_name); -#else - kr_trackers_manager::Tracker *c = tracker_loader_.createClassInstance(tracker_name); -#endif - c->Initialize(priv_nh); - tracker_map_.insert(std::make_pair(tracker_name, c)); + auto ptr = tracker_loader_.createSharedInstance(name); + ptr->Initialize(weak_node); + std::cout << "Loaded successfully\n"; + tracker_map_.insert(std::make_pair(name, ptr)); } - catch(pluginlib::LibraryLoadException &e) + catch (const pluginlib::LibraryLoadException& e) { - NODELET_ERROR_STREAM("Could not load library for the tracker " << tracker_name << ": " << e.what()); + RCLCPP_ERROR(this->get_logger(), "Library Load Exception: %s", e.what()); } - catch(pluginlib::CreateClassException &e) + catch (const pluginlib::CreateClassException& e) { - NODELET_ERROR_STREAM("Could not create an instance of the tracker " << tracker_name << ": " << e.what()); + RCLCPP_ERROR(this->get_logger(), "Create Class Exception: %s", e.what()); } } - pub_cmd_ = priv_nh.advertise("cmd", 10); - pub_status_ = priv_nh.advertise("status", 10); + pub_cmd_ = this->create_publisher("~/cmd", 10); + pub_status_ = this->create_publisher("~/status", 10); + + // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + sub_odom_ = this->create_subscription("~/odom", qos, std::bind(&TrackersManager::odom_callback, this, std::placeholders::_1)); - sub_odom_ = priv_nh.subscribe("odom", 10, &TrackersManager::odom_callback, this, ros::TransportHints().tcpNoDelay()); + srv_tracker_ = this->create_service("~/transition", std::bind(&TrackersManager::transition_callback, this, std::placeholders::_1, std::placeholders::_2)); - srv_tracker_ = priv_nh.advertiseService("transition", &TrackersManager::transition_callback, this); + return LifecycleCallbackReturn::SUCCESS; } -void TrackersManager::odom_callback(const nav_msgs::Odometry::ConstPtr &msg) +LifecycleCallbackReturn TrackersManager::on_activate(const rclcpp_lifecycle::State &previous_state) { - std::map::iterator it; + RCLCPP_INFO(this->get_logger(), "Activating TrackersManager."); + rclcpp_lifecycle::LifecycleNode::on_activate(previous_state); + return LifecycleCallbackReturn::SUCCESS; +} + +void TrackersManager::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + std::map>::iterator it; for(it = tracker_map_.begin(); it != tracker_map_.end(); it++) { if(it->second == active_tracker_) { cmd_ = it->second->update(msg); if(cmd_ != NULL) - pub_cmd_.publish(cmd_); + { + // TODO: make cmd_ a unique pointer + pub_cmd_->publish(*cmd_); + } - kr_tracker_msgs::TrackerStatus::Ptr status_msg(new kr_tracker_msgs::TrackerStatus); + auto status_msg = std::make_unique(); status_msg->header.stamp = msg->header.stamp; status_msg->tracker = it->first; status_msg->status = it->second->status(); - pub_status_.publish(status_msg); + pub_status_->publish(std::move(status_msg)); } else { @@ -114,31 +130,31 @@ void TrackersManager::odom_callback(const nav_msgs::Odometry::ConstPtr &msg) } } -bool TrackersManager::transition_callback(kr_tracker_msgs::Transition::Request &req, - kr_tracker_msgs::Transition::Response &res) +void TrackersManager::transition_callback(const kr_tracker_msgs::srv::Transition::Request::SharedPtr req, const kr_tracker_msgs::srv::Transition::Response::SharedPtr res) { - const std::map::iterator it = tracker_map_.find(req.tracker); + const std::map>::iterator it = tracker_map_.find(req->tracker); if(it == tracker_map_.end()) { - res.success = false; - res.message = std::string("Cannot find tracker ") + req.tracker + std::string(", cannot transition"); - NODELET_WARN_STREAM(res.message); - return true; + res->success = false; + res->message = std::string("Cannot find tracker ") + req->tracker + std::string(", cannot transition"); + RCLCPP_WARN_STREAM(this->get_logger(), res->message); + return; } if(active_tracker_ == it->second) { - res.success = true; - res.message = std::string("Tracker ") + req.tracker + std::string(" already active"); - NODELET_INFO_STREAM(res.message); - return true; + res->success = true; + res->message = std::string("Tracker ") + req->tracker + std::string(" already active"); + RCLCPP_INFO_STREAM(this->get_logger(), res->message); + return; } + // TODO: change arguments of Activate function to take in a ConstSharedPtr if(!it->second->Activate(cmd_)) { - res.success = false; - res.message = std::string("Failed to activate tracker ") + req.tracker + std::string(", cannot transition"); - NODELET_WARN_STREAM(res.message); - return true; + res->success = true; + res->message = std::string("Failed to activate tracker ") + req->tracker + std::string(", cannot transition"); + RCLCPP_INFO_STREAM(this->get_logger(), res->message); + return; } if(active_tracker_ != NULL) @@ -147,10 +163,16 @@ bool TrackersManager::transition_callback(kr_tracker_msgs::Transition::Request & } active_tracker_ = it->second; - res.success = true; - res.message = std::string("Successfully activated tracker ") + req.tracker; - return true; + res->success = true; + res->message = std::string("Successfully activated tracker ") + req->tracker; +} + +LifecycleCallbackReturn TrackersManager::on_shutdown(const rclcpp_lifecycle::State &previous_state) +{ + (void)previous_state; + RCLCPP_INFO(this->get_logger(), "Shutting down Trackers Manager"); + return LifecycleCallbackReturn::SUCCESS; } -#include -PLUGINLIB_EXPORT_CLASS(TrackersManager, nodelet::Nodelet); +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(TrackersManager) From 7f8f04050769e3dcf1ce60f7f2085af4c483e4ba Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Thu, 17 Oct 2024 22:54:59 -0500 Subject: [PATCH 17/64] kr_mav_manager migrated to ROS2 humble format --- kr_mav_manager/CMakeLists.txt | 118 ++-- .../include/kr_mav_manager/manager.h | 194 ------- .../include/kr_mav_manager/manager.hpp | 217 ++++++++ .../kr_mav_manager/mav_manager_services.h | 195 ------- .../kr_mav_manager/mav_manager_services.hpp | 248 +++++++++ kr_mav_manager/package.xml | 32 +- kr_mav_manager/src/manager.cpp | 510 ++++++++++-------- kr_mav_manager/src/mav_services.cpp | 14 +- kr_mav_manager/src/sample_sub.cpp | 2 + kr_mav_manager/srv/Circle.srv | 8 +- kr_mav_manager/srv/CompoundLissajous.srv | 2 +- kr_mav_manager/srv/GoalTimed.srv | 6 +- kr_mav_manager/srv/Lissajous.srv | 2 +- kr_mav_manager/srv/Vec4.srv | 2 +- 14 files changed, 858 insertions(+), 692 deletions(-) delete mode 100644 kr_mav_manager/include/kr_mav_manager/manager.h create mode 100644 kr_mav_manager/include/kr_mav_manager/manager.hpp delete mode 100644 kr_mav_manager/include/kr_mav_manager/mav_manager_services.h create mode 100644 kr_mav_manager/include/kr_mav_manager/mav_manager_services.hpp diff --git a/kr_mav_manager/CMakeLists.txt b/kr_mav_manager/CMakeLists.txt index 8c404d00..19eaa47c 100644 --- a/kr_mav_manager/CMakeLists.txt +++ b/kr_mav_manager/CMakeLists.txt @@ -1,70 +1,78 @@ cmake_minimum_required(VERSION 3.10) project(kr_mav_manager) -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) endif() -set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) -find_package( - catkin REQUIRED - COMPONENTS roscpp - nav_msgs - sensor_msgs - geometry_msgs - kr_mav_msgs - kr_tracker_msgs - std_msgs - std_srvs - message_generation - actionlib) -find_package(Eigen3 REQUIRED NO_MODULE) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -add_service_files( - DIRECTORY - srv - FILES - Vec4.srv - GoalTimed.srv - Circle.srv - Lissajous.srv - CompoundLissajous.srv) -generate_messages() +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(kr_mav_msgs REQUIRED) +find_package(kr_tracker_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - roscpp - std_msgs - nav_msgs - sensor_msgs - geometry_msgs - kr_mav_msgs - kr_tracker_msgs - message_runtime - DEPENDS - EIGEN3) +rosidl_generate_interfaces(${PROJECT_NAME} +"srv/Circle.srv" +"srv/CompoundLissajous.srv" +"srv/GoalTimed.srv" +"srv/Lissajous.srv" +"srv/Vec4.srv" +DEPENDENCIES builtin_interfaces +) -add_library(${PROJECT_NAME} src/manager.cpp) -target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} Eigen3::Eigen) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +include_directories(include/) -add_executable(mav_services src/mav_services.cpp) -target_link_libraries(mav_services PRIVATE ${PROJECT_NAME}) +## Create library +add_library(manager src/manager.cpp) + +ament_export_targets(manager HAS_LIBRARY_TARGET) +ament_target_dependencies(manager rclcpp rclcpp_action nav_msgs std_msgs sensor_msgs kr_tracker_msgs kr_mav_msgs tf2 tf2_geometry_msgs) +target_link_libraries(manager Eigen3::Eigen) +ament_export_dependencies(manager rclcpp rclcpp_action nav_msgs std_msgs sensor_msgs kr_mav_msgs kr_tracker_msgs Eigen3::Eigen) install( - TARGETS ${PROJECT_NAME} mav_services - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + TARGETS manager + EXPORT manager + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) +## + +add_executable(mav_services src/mav_services.cpp) +ament_target_dependencies(mav_services std_srvs) +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") # to link local msg/srv/action definitions (humble and above) +target_link_libraries(mav_services manager "${cpp_typesupport_target}") + +install(TARGETS +mav_services +DESTINATION lib/${PROJECT_NAME} +) + +ament_export_dependencies(rosidl_default_runtime) -install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +ament_package() diff --git a/kr_mav_manager/include/kr_mav_manager/manager.h b/kr_mav_manager/include/kr_mav_manager/manager.h deleted file mode 100644 index 6db59677..00000000 --- a/kr_mav_manager/include/kr_mav_manager/manager.h +++ /dev/null @@ -1,194 +0,0 @@ -#ifndef MANAGER_H -#define MANAGER_H - -// Standard C++ -#include -#include -#include - -// ROS related -#include -#include -#include -#include -#include - -// kr_mav_control -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace kr_mav_manager -{ -class MAVManager -{ - public: - // Typedefs - typedef Eigen::Vector2f Vec2; - typedef Eigen::Vector3f Vec3; - typedef Eigen::Vector4f Vec4; - typedef Eigen::Quaternionf Quat; - - enum Status - { - INIT, - MOTORS_OFF, - IDLE, - ELAND, - ESTOP, - FLYING - }; - - MAVManager(std::string ns = ""); - - // Accessors - Vec3 pos() { return pos_; } - Vec3 vel() { return vel_; } - Vec3 home() { return home_; } - float yaw() { return yaw_; } - float home_yaw() { return home_yaw_; } - float mass() { return mass_; } - std::string active_tracker() { return active_tracker_; } - bool need_imu() { return need_imu_; } - bool need_odom() { return need_odom_; } - Status status() { return status_; } - - // Mutators - bool set_mass(float m); - void set_need_imu(bool flag) { need_imu_ = flag; } // TODO: Consider not allowing this to be toggled after takeoff - void set_need_odom(bool flag) { need_odom_ = flag; } // TODO: Consider not allowing this to be toggled after takeoff - void set_use_attitude_safety_catch(bool flag) { use_attitude_safety_catch_ = flag; } - - // Home Methods - bool setHome(); // Uses the current position and yaw - bool goHome(); - bool land(); - - // Movement - bool takeoff(); - - bool goTo(float x, float y, float z, float yaw, float v_des = 0.0f, float a_des = 0.0f, bool relative = false); - bool goTo(Vec4 xyz_yaw, Vec2 v_and_a_des = Vec2::Zero()); - bool goTo(Vec3 xyz, float yaw, Vec2 v_and_a_des = Vec2::Zero()); - bool goTo(Vec3 xyz, Vec2 v_and_a_des = Vec2::Zero()); // Uses Current yaw - - bool goToTimed(float x, float y, float z, float yaw, float v_des = 0.0f, float a_des = 0.0f, bool relative = false, - ros::Duration duration = ros::Duration(0), ros::Time start_time = ros::Time::now()); - - bool setDesVelInWorldFrame(float x, float y, float z, float yaw, bool use_position_feedback = false); - bool setDesVelInBodyFrame(float x, float y, float z, float yaw, bool use_position_feedback = false); - - // Yaw control - bool goToYaw(float); - - bool circle(float Ax, float Ay, float T, float duration); - - // Lissajous Control - bool lissajous(float x_amp, float y_amp, float z_amp, float yaw_amp, float x_num_periods, float y_num_periods, - float z_num_periods, float yaw_num_periods, float period, float num_cycles, float ramp_time); - - // Compound Lissajous Control - bool compound_lissajous(float x_amp[2], float y_amp[2], float z_amp[2], float yaw_amp[2], float x_num_periods[2], - float y_num_periods[2], float z_num_periods[2], float yaw_num_periods[2], float period[2], - float num_cycles[2], float ramp_time[2]); - - // Direct low-level control - bool setPositionCommand(const kr_mav_msgs::PositionCommand &cmd); - bool setSO3Command(const kr_mav_msgs::SO3Command &cmd); - bool setTRPYCommand(const kr_mav_msgs::TRPYCommand &cmd); - bool useNullTracker(); - - // Monitoring - bool have_recent_odom(), have_recent_imu(), have_recent_output_data(); - float voltage() { return voltage_; } - float pressure_height() { return pressure_height_; } - float pressure_dheight() { return pressure_dheight_; } - std::array magnetic_field() { return magnetic_field_; } - std::array radio() { return radio_; } - - // Safety - bool hover(); - bool ehover(); - bool set_motors(bool); - bool motors() { return motors_; } - bool eland(); - bool estop(); - - bool transition(const std::string &tracker_str); - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; - - private: - typedef actionlib::SimpleActionClient ClientType; - typedef actionlib::SimpleActionClient CircleClientType; - typedef actionlib::SimpleActionClient LissajousClientType; - typedef actionlib::SimpleActionClient CompoundLissajousClientType; - - ros::NodeHandle nh_; - ros::NodeHandle priv_nh_; - - void tracker_done_callback(const actionlib::SimpleClientGoalState &state, - const kr_tracker_msgs::LineTrackerResultConstPtr &result); - void circle_tracker_done_callback(const actionlib::SimpleClientGoalState &state, - const kr_tracker_msgs::CircleTrackerResultConstPtr &result); - void lissajous_tracker_done_callback(const actionlib::SimpleClientGoalState &state, - const kr_tracker_msgs::LissajousTrackerResultConstPtr &result); - void lissajous_adder_done_callback(const actionlib::SimpleClientGoalState &state, - const kr_tracker_msgs::LissajousAdderResultConstPtr &result); - - void odometry_cb(const nav_msgs::Odometry::ConstPtr &msg); - void imu_cb(const sensor_msgs::Imu::ConstPtr &msg); - void output_data_cb(const kr_mav_msgs::OutputData::ConstPtr &msg); - void heartbeat_cb(const std_msgs::Empty::ConstPtr &msg); - void tracker_status_cb(const kr_tracker_msgs::TrackerStatus::ConstPtr &msg); - void heartbeat(); - - std::string active_tracker_; - - Status status_; - - ros::Time last_odom_t_, last_imu_t_, last_output_data_t_, last_heartbeat_t_; - - Vec3 pos_, vel_; - float mass_; - Quat odom_q_, imu_q_; - float yaw_, yaw_dot_; - float takeoff_height_; - float max_attitude_angle_; - float odom_timeout_; - - Vec3 home_, goal_; - float home_yaw_; - - bool need_imu_, need_output_data_, need_odom_, use_attitude_safety_catch_; - bool home_set_, motors_; - float voltage_, pressure_height_, pressure_dheight_; - std::array magnetic_field_; - std::array radio_; - - // Actionlibs - ClientType line_tracker_distance_client_; - ClientType line_tracker_min_jerk_client_; - CircleClientType circle_tracker_client_; - LissajousClientType lissajous_tracker_client_; - CompoundLissajousClientType lissajous_adder_client_; - - // Publishers - ros::Publisher pub_motors_, pub_estop_, pub_goal_yaw_, pub_goal_velocity_, pub_so3_command_, pub_trpy_command_, - pub_position_command_, pub_status_, pub_pwm_command_; - - // Subscribers - ros::Subscriber odom_sub_, imu_sub_, output_data_sub_, heartbeat_sub_, tracker_status_sub_; - - // Services - ros::ServiceClient srv_transition_; -}; - -} // namespace kr_mav_manager -#endif /* MANAGER_H */ diff --git a/kr_mav_manager/include/kr_mav_manager/manager.hpp b/kr_mav_manager/include/kr_mav_manager/manager.hpp new file mode 100644 index 00000000..ab2553a4 --- /dev/null +++ b/kr_mav_manager/include/kr_mav_manager/manager.hpp @@ -0,0 +1,217 @@ +#pragma once + +// Standard C++ Libraries +#include +#include +#include + +// ROS2 related +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/empty.hpp" +#include "std_msgs/msg/u_int8.hpp" + +// kr_mav_control +#include "kr_mav_msgs/msg/output_data.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_mav_msgs/msg/so3_command.hpp" +#include "kr_mav_msgs/msg/trpy_command.hpp" +#include "kr_tracker_msgs/action/circle_tracker.hpp" +#include "kr_tracker_msgs/action/line_tracker.hpp" +#include "kr_tracker_msgs/action/lissajous_adder.hpp" +#include "kr_tracker_msgs/action/lissajous_tracker.hpp" +#include "kr_tracker_msgs/msg/tracker_status.hpp" +#include "kr_tracker_msgs/msg/velocity_goal.hpp" +#include "kr_tracker_msgs/srv/transition.hpp" + +namespace kr_mav_manager +{ + +using LineTracker = kr_tracker_msgs::action::LineTracker; +using LineTrackerGoalHandle = rclcpp_action::ClientGoalHandle; +using CircleTracker = kr_tracker_msgs::action::CircleTracker; +using CircleTrackerGoalHandle = rclcpp_action::ClientGoalHandle; +using LissajousTracker = kr_tracker_msgs::action::LissajousTracker; +using LissajousTrackerGoalHandle = rclcpp_action::ClientGoalHandle; +using LissajousAdder = kr_tracker_msgs::action::LissajousAdder; +using LissajousAdderGoalHandle = rclcpp_action::ClientGoalHandle; + +class MAVManager : public rclcpp::Node +{ + public: + // Typedefs + typedef Eigen::Vector2f Vec2; + typedef Eigen::Vector3f Vec3; + typedef Eigen::Vector4f Vec4; + typedef Eigen::Quaternionf Quat; + + enum Status + { + INIT, + MOTORS_OFF, + IDLE, + ELAND, + ESTOP, + FLYING + }; + + MAVManager(); + + // Accessors + Vec3 pos() { return pos_; } + Vec3 vel() { return vel_; } + Vec3 home() { return home_; } + float yaw() { return yaw_; } + float home_yaw() { return home_yaw_; } + float mass() { return mass_; } + std::string active_tracker() { return active_tracker_; } + bool need_imu() { return need_imu_; } + bool need_odom() { return need_odom_; } + Status status() { return status_; } + + // Mutators + bool set_mass(float m); + void set_need_imu(bool flag) { need_imu_ = flag; } // TODO: Consider not allowing this to be toggled after takeoff + void set_need_odom(bool flag) { need_odom_ = flag; } // TODO: Consider not allowing this to be toggled after takeoff + void set_use_attitude_safety_catch(bool flag) { use_attitude_safety_catch_ = flag; } + + // Home Methods + bool setHome(); // Uses the current position and yaw + bool goHome(); + bool land(); + + // Movement + bool takeoff(); + + bool goTo(float x, float y, float z, float yaw, float v_des = 0.0f, float a_des = 0.0f, bool relative = false); + bool goTo(Vec4 xyz_yaw, Vec2 v_and_a_des = Vec2::Zero()); + bool goTo(Vec3 xyz, float yaw, Vec2 v_and_a_des = Vec2::Zero()); + bool goTo(Vec3 xyz, Vec2 v_and_a_des = Vec2::Zero()); // Uses current yaw + + bool goToTimed(float x, float y, float z, float yaw, float v_des = 0.0f, float a_des = 0.0f, bool relative = false, + rclcpp::Duration duration = rclcpp::Duration(0, 0), rclcpp::Time start_time = rclcpp::Time(0)); + + bool setDesVelInWorldFrame(float x, float y, float z, float yaw, bool use_position_feedback = false); + bool setDesVelInBodyFrame(float x, float y, float z, float yaw, bool use_position_feedback = false); + + // Yaw Control + bool goToYaw(float); + + bool circle(float Ax, float Ay, float T, float duration); + + // Lissajous Control + bool lissajous(float x_amp, float y_amp, float z_amp, float yaw_amp, float x_num_periods, float y_num_periods, + float z_num_periods, float yaw_num_periods, float period, float num_cycles, float ramp_time); + + // Compound Lissajous Control + bool compound_lissajous(float x_amp[2], float y_amp[2], float z_amp[2], float yaw_amp[2], float x_num_periods[2], + float y_num_periods[2], float z_num_periods[2], float yaw_num_periods[2], float period[2], + float num_cycles[2], float ramp_time[2]); + + // Direct low-level control + bool setPositionCommand(const kr_mav_msgs::msg::PositionCommand &msg); + bool setSO3Command(const kr_mav_msgs::msg::SO3Command &msg); + bool setTRPYCommand(const kr_mav_msgs::msg::TRPYCommand &msg); + bool useNullTracker(); + + // Monitoring + bool have_recent_odom(); + bool have_recent_imu(); + bool have_recent_output_data(); + float voltage() { return voltage_; } + float pressure_height() { return pressure_height_; } + float pressure_dheight() { return pressure_dheight_; } + std::array magnetic_field() { return magnetic_field_; } + std::array radio() { return radio_; } + + // Safety + bool hover(); + bool ehover(); + bool set_motors(bool); + bool motors() { return motors_; } + bool eland(); + bool estop(); + + bool transition(const std::string &tracker_str); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + typedef rclcpp_action::Client::SharedPtr ClientType; + typedef rclcpp_action::Client::SharedPtr CircleClientType; + typedef rclcpp_action::Client::SharedPtr LissajousClientType; + typedef rclcpp_action::Client::SharedPtr CompoundLissajousClientType; + + void tracker_done_callback(const LineTrackerGoalHandle::WrappedResult &result); + void circle_tracker_done_callback(const CircleTrackerGoalHandle::WrappedResult &result); + void lissajous_tracker_done_callback(const LissajousTrackerGoalHandle::WrappedResult &result); + void lissajous_adder_done_callback(const LissajousAdderGoalHandle::WrappedResult &result); + + void odometry_cb(nav_msgs::msg::Odometry::ConstSharedPtr msg); + void imu_cb(sensor_msgs::msg::Imu::ConstSharedPtr msg); + void output_data_cb(kr_mav_msgs::msg::OutputData::ConstSharedPtr msg); + void heartbeat_cb(std_msgs::msg::Empty::ConstSharedPtr msg); + void tracker_status_cb(kr_tracker_msgs::msg::TrackerStatus::ConstSharedPtr msg); + void heartbeat(); + + std::string active_tracker_; + + Status status_; + + rclcpp::Time last_odom_t_, last_imu_t_, last_output_data_t_, last_heartbeat_t_; + + Vec3 pos_, vel_; + float mass_; + Quat odom_q_, imu_q_; + float yaw_, yaw_dot_; + float takeoff_height_; + float max_attitude_angle_; + float odom_timeout_; + + Vec3 home_, goal_; + float home_yaw_; + + bool need_imu_, need_output_data_, need_odom_, use_attitude_safety_catch_; + bool home_set_, motors_; + float voltage_, pressure_height_, pressure_dheight_; + std::array magnetic_field_; + std::array radio_; + + // Action Clients + ClientType line_tracker_distance_client_; + ClientType line_tracker_min_jerk_client_; + CircleClientType circle_tracker_client_; + LissajousClientType lissajous_tracker_client_; + CompoundLissajousClientType lissajous_adder_client_; + + // Publishers + rclcpp::Publisher::SharedPtr pub_motors_; + rclcpp::Publisher::SharedPtr pub_estop_; + rclcpp::Publisher::SharedPtr pub_so3_command_; + rclcpp::Publisher::SharedPtr pub_trpy_command_; + rclcpp::Publisher::SharedPtr pub_position_command_; + rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_goal_velocity_; + // pub_goal_yaw_ and pub_pwm_command_ defined in ros1 package but not being used + + // Subscribers + rclcpp::Subscription::SharedPtr odom_sub_; // odometry_cb + rclcpp::Subscription::SharedPtr heartbeat_sub_; // heartbeat_cb + rclcpp::Subscription::SharedPtr tracker_status_sub_; // tracker_status_cb + rclcpp::Subscription::SharedPtr imu_sub_; // imu_cb + rclcpp::Subscription::SharedPtr output_data_sub_; + + // Services + rclcpp::Client::SharedPtr srv_transition_; + + // Helper variable + std::map result_status = {{rclcpp_action::ResultCode::SUCCEEDED, "SUCCEEDED"}, + {rclcpp_action::ResultCode::ABORTED, "ABORTED"}, + {rclcpp_action::ResultCode::CANCELED, "CANCELED"}, + {rclcpp_action::ResultCode::UNKNOWN, "UNKNOWN"}}; +}; + +} // namespace kr_mav_manager diff --git a/kr_mav_manager/include/kr_mav_manager/mav_manager_services.h b/kr_mav_manager/include/kr_mav_manager/mav_manager_services.h deleted file mode 100644 index 444ad79b..00000000 --- a/kr_mav_manager/include/kr_mav_manager/mav_manager_services.h +++ /dev/null @@ -1,195 +0,0 @@ -#ifndef MAV_MANAGER_SERVICES_H -#define MAV_MANAGER_SERVICES_H - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace kr_mav_manager -{ -class MAVManagerServices -{ - public: - std::vector srvs_; - - bool motors_cb(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) - { - res.success = mav->set_motors(req.data); - res.message = "Motors "; - res.message += req.data ? "on" : "off"; - if(res.success) - last_cb_ = "motors"; - return true; - } - bool takeoff_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { - res.success = mav->takeoff(); - res.message = "Takeoff"; - if(res.success) - last_cb_ = "takeoff"; - return true; - } - bool goHome_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { - res.success = mav->goHome(); - res.message = "Going home"; - if(res.success) - last_cb_ = "goHome"; - return true; - } - bool goTo_cb(kr_mav_manager::Vec4::Request &req, kr_mav_manager::Vec4::Response &res) - { - res.success = mav->goTo(req.goal[0], req.goal[1], req.goal[2], req.goal[3]); - res.message = "Going To..."; - if(res.success) - last_cb_ = "goTo"; - return true; - } - bool goToTimed_cb(kr_mav_manager::GoalTimed::Request &req, kr_mav_manager::GoalTimed::Response &res) - { - res.success = mav->goToTimed(req.goal[0], req.goal[1], req.goal[2], req.goal[3], 0.0f, 0.0f, false, req.duration, - req.t_start); - res.message = "Going To Timed..."; - if(res.success) - last_cb_ = "goToTimed"; - return res.success; - } - bool goToRelative_cb(kr_mav_manager::Vec4::Request &req, kr_mav_manager::Vec4::Response &res) - { - res.success = mav->goTo(req.goal[0], req.goal[1], req.goal[2], req.goal[3], 0.0f, 0.0f, true); - res.message = "Going To Relative Position..."; - if(res.success) - last_cb_ = "goToRelative"; - return res.success; - } - bool setDesVelInWorldFrame_cb(kr_mav_manager::Vec4::Request &req, kr_mav_manager::Vec4::Response &res) - { - res.success = mav->setDesVelInWorldFrame(req.goal[0], req.goal[1], req.goal[2], req.goal[3], true); - res.message = "World Velocity"; - if(res.success) - last_cb_ = "setDesVelInWorldFrmae"; - return true; - } - bool setDesVelInBodyFrame_cb(kr_mav_manager::Vec4::Request &req, kr_mav_manager::Vec4::Response &res) - { - res.success = mav->setDesVelInBodyFrame(req.goal[0], req.goal[1], req.goal[2], req.goal[3], true); - res.message = "Body Velocity"; - if(res.success) - last_cb_ = "setDesVelInBodyFrame"; - return true; - } - bool circle_cb(kr_mav_manager::Circle::Request &req, kr_mav_manager::Circle::Response &res) - { - res.success = mav->circle(req.Ax, req.Ay, req.T, req.duration); - res.message = "Circling motion"; - if(res.success) - last_cb_ = "circle"; - return true; - } - bool lissajous_cb(kr_mav_manager::Lissajous::Request &req, kr_mav_manager::Lissajous::Response &res) - { - res.success = 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) - last_cb_ = "lissajous"; - return true; - } - bool compound_lissajous_cb(kr_mav_manager::CompoundLissajous::Request &req, - kr_mav_manager::CompoundLissajous::Response &res) - { - float x_amp[2] = {req.x_amp[0], req.x_amp[1]}; - float y_amp[2] = {req.y_amp[0], req.y_amp[1]}; - float z_amp[2] = {req.z_amp[0], req.z_amp[1]}; - float yaw_amp[2] = {req.yaw_amp[0], req.yaw_amp[1]}; - float x_num_periods[2] = {req.x_num_periods[0], req.x_num_periods[1]}; - float y_num_periods[2] = {req.y_num_periods[0], req.y_num_periods[1]}; - float z_num_periods[2] = {req.z_num_periods[0], req.z_num_periods[1]}; - float yaw_num_periods[2] = {req.yaw_num_periods[0], req.yaw_num_periods[1]}; - float period[2] = {req.period[0], req.period[1]}; - float num_cycles[2] = {req.num_cycles[0], req.num_cycles[1]}; - float ramp_time[2] = {req.ramp_time[0], req.ramp_time[1]}; - res.success = mav->compound_lissajous(x_amp, y_amp, z_amp, yaw_amp, x_num_periods, y_num_periods, z_num_periods, - yaw_num_periods, period, num_cycles, ramp_time); - res.message = "Compound Lissajous motion"; - if(res.success) - last_cb_ = "compound_lissajous"; - return true; - } - bool hover_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { - res.success = mav->hover(); - res.message = "Hover"; - if(res.success) - last_cb_ = "hover"; - return true; - } - bool ehover_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { - res.success = mav->ehover(); - res.message = "Emergency Hover"; - if(res.success) - last_cb_ = "ehover"; - return true; - } - bool land_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { - res.success = mav->land(); - res.message = "Landing"; - if(res.success) - last_cb_ = "land"; - return true; - } - bool eland_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { - res.success = mav->eland(); - res.message = "Emergency Landing"; - if(res.success) - last_cb_ = "eland"; - return true; - } - bool estop_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { - res.success = mav->estop(); - res.message = "Emergency Stop"; - if(res.success) - last_cb_ = "estop"; - return true; - } - - // Constructor - MAVManagerServices(std::shared_ptr m) : nh_("~"), mav(m), last_cb_("") - { - srvs_.push_back(nh_.advertiseService("motors", &MAVManagerServices::motors_cb, this)); - srvs_.push_back(nh_.advertiseService("takeoff", &MAVManagerServices::takeoff_cb, this)); - srvs_.push_back(nh_.advertiseService("goHome", &MAVManagerServices::goHome_cb, this)); - srvs_.push_back(nh_.advertiseService("goTo", &MAVManagerServices::goTo_cb, this)); - srvs_.push_back(nh_.advertiseService("goToTimed", &MAVManagerServices::goToTimed_cb, this)); - srvs_.push_back(nh_.advertiseService("goToRelative", &MAVManagerServices::goToRelative_cb, this)); - srvs_.push_back(nh_.advertiseService("setDesVelInWorldFrame", &MAVManagerServices::setDesVelInWorldFrame_cb, this)); - srvs_.push_back(nh_.advertiseService("setDesVelInBodyFrame", &MAVManagerServices::setDesVelInBodyFrame_cb, this)); - srvs_.push_back(nh_.advertiseService("circle", &MAVManagerServices::circle_cb, this)); - srvs_.push_back(nh_.advertiseService("lissajous", &MAVManagerServices::lissajous_cb, this)); - srvs_.push_back(nh_.advertiseService("compound_lissajous", &MAVManagerServices::compound_lissajous_cb, this)); - srvs_.push_back(nh_.advertiseService("hover", &MAVManagerServices::hover_cb, this)); - srvs_.push_back(nh_.advertiseService("ehover", &MAVManagerServices::ehover_cb, this)); - srvs_.push_back(nh_.advertiseService("land", &MAVManagerServices::land_cb, this)); - srvs_.push_back(nh_.advertiseService("eland", &MAVManagerServices::eland_cb, this)); - srvs_.push_back(nh_.advertiseService("estop", &MAVManagerServices::estop_cb, this)); - } - - protected: - ros::NodeHandle nh_; - - // Let's make an MAV pointer - std::shared_ptr mav; - - std::string last_cb_; -}; -} // namespace kr_mav_manager -#endif /* MAV_MANAGER_SERVICES_H */ 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 new file mode 100644 index 00000000..9f9e82a3 --- /dev/null +++ b/kr_mav_manager/include/kr_mav_manager/mav_manager_services.hpp @@ -0,0 +1,248 @@ +#pragma once + +#include "kr_mav_manager/manager.hpp" +#include "kr_mav_manager/srv/circle.hpp" +#include "kr_mav_manager/srv/compound_lissajous.hpp" +#include "kr_mav_manager/srv/goal_timed.hpp" +#include "kr_mav_manager/srv/lissajous.hpp" +#include "kr_mav_manager/srv/vec4.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace kr_mav_manager +{ + +using namespace std::placeholders; + +class MAVManagerServices +{ + public: + void motors_cb(const std_srvs::srv::SetBool::Request::SharedPtr req, + const std_srvs::srv::SetBool::Response::SharedPtr res) + { + res->success = mav->set_motors(req->data); + res->message = "Motors "; + res->message += req->data ? "on" : "off"; + if(res->success) + last_cb_ = "motors"; + } + + void takeoff_cb(const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res) + { + (void)req; + res->success = mav->takeoff(); + res->message = "Takeoff"; + if(res->success) + last_cb_ = "takeoff"; + } + + void goHome_cb(const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res) + { + (void)req; + res->success = mav->goHome(); + res->message = "Going Home"; + if(res->success) + last_cb_ = "goHome"; + } + + void goTo_cb(const kr_mav_manager::srv::Vec4::Request::SharedPtr req, + const kr_mav_manager::srv::Vec4::Response::SharedPtr res) + { + res->success = mav->goTo(req->goal[0], req->goal[1], req->goal[2], req->goal[4]); + res->message = "Going To..."; + if(res->success) + last_cb_ = "goTo"; + } + + void goToTimed_cb(const kr_mav_manager::srv::GoalTimed::Request::SharedPtr req, + const kr_mav_manager::srv::GoalTimed::Response::SharedPtr res) + { + res->success = mav->goToTimed(req->goal[0], req->goal[1], req->goal[2], req->goal[3], 0.0f, 0.0f, false, + req->duration, req->t_start); + res->message = "Going To Timed..."; + if(res->success) + last_cb_ = "goToTimed"; + } + + void goToRelative_cb(const kr_mav_manager::srv::Vec4::Request::SharedPtr req, + const kr_mav_manager::srv::Vec4::Response::SharedPtr res) + { + res->success = mav->goTo(req->goal[0], req->goal[1], req->goal[2], req->goal[3], 0.0f, 0.0f, true); + res->message = "Going To Relative Position..."; + if(res->success) + last_cb_ = "goToRelative"; + } + + void setDesVelInWorldFrame_cb(const kr_mav_manager::srv::Vec4::Request::SharedPtr req, + const kr_mav_manager::srv::Vec4::Response::SharedPtr res) + { + res->success = mav->setDesVelInWorldFrame(req->goal[0], req->goal[1], req->goal[2], req->goal[3], true); + res->message = "World Velocity"; + if(res->success) + last_cb_ = "setDesVelInWorldFrame"; + } + + void setDesVelInBodyFrame_cb(const kr_mav_manager::srv::Vec4::Request::SharedPtr req, + const kr_mav_manager::srv::Vec4::Response::SharedPtr res) + { + res->success = mav->setDesVelInBodyFrame(req->goal[0], req->goal[1], req->goal[2], req->goal[3], true); + res->message = "Body Velocity"; + if(res->success) + last_cb_ = "setDesVelInBodyFrame"; + } + + 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->message = "Circling motion"; + if(res->success) + last_cb_ = "circle"; + } + + void lissajous_cb(const kr_mav_manager::srv::Lissajous::Request::SharedPtr req, + 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, + req->z_num_periods, req->yaw_num_periods, req->period, req->num_cycles, req->ramp_time); + res->message = "Lissajous motion"; + if(res->success) + last_cb_ = "lissajous"; + } + + void compound_lissajous_cb(const kr_mav_manager::srv::CompoundLissajous::Request::SharedPtr req, + const kr_mav_manager::srv::CompoundLissajous::Response::SharedPtr res) + { + float x_amp[2] = {req->x_amp[0], req->x_amp[1]}; + float y_amp[2] = {req->y_amp[0], req->y_amp[1]}; + float z_amp[2] = {req->z_amp[0], req->z_amp[1]}; + float yaw_amp[2] = {req->yaw_amp[0], req->yaw_amp[1]}; + float x_num_periods[2] = {req->x_num_periods[0], req->x_num_periods[1]}; + float y_num_periods[2] = {req->y_num_periods[0], req->y_num_periods[1]}; + float z_num_periods[2] = {req->z_num_periods[0], req->z_num_periods[1]}; + float yaw_num_periods[2] = {req->yaw_num_periods[0], req->yaw_num_periods[1]}; + float period[2] = {req->period[0], req->period[1]}; + float num_cycles[2] = {req->num_cycles[0], req->num_cycles[1]}; + float ramp_time[2] = {req->ramp_time[0], req->ramp_time[1]}; + res->success = mav->compound_lissajous(x_amp, y_amp, z_amp, yaw_amp, x_num_periods, y_num_periods, z_num_periods, + yaw_num_periods, period, num_cycles, ramp_time); + res->message = "Compound Lissajous motion"; + if(res->success) + last_cb_ = "compound_lissajous"; + } + + void hover_cb(const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res) + { + (void)req; + res->success = mav->hover(); + res->message = "Hover"; + if(res->success) + last_cb_ = "hover"; + } + + void ehover_cb(const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res) + { + (void)req; + res->success = mav->ehover(); + res->message = "Emergency Hover"; + if(res->success) + last_cb_ = "ehover"; + } + + void land_cb(const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res) + { + (void)req; + res->success = mav->land(); + res->message = "Landing"; + if(res->success) + last_cb_ = "land"; + } + + void eland_cb(const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res) + { + (void)req; + res->success = mav->eland(); + res->message = "Emergency Landing"; + if(res->success) + last_cb_ = "eland"; + } + + void estop_cb(const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res) + { + (void)req; + res->success = mav->estop(); + res->message = "Emergency Stop"; + if(res->success) + last_cb_ = "estop"; + } + + // Constructor + MAVManagerServices(std::shared_ptr m) : mav(m), last_cb_("") + { + motors_srv_ = mav->create_service("~/motors", + std::bind(&MAVManagerServices::motors_cb, this, _1, _2)); + takeoff_srv_ = mav->create_service( + "~/takeoff", std::bind(&MAVManagerServices::takeoff_cb, this, _1, _2)); + goHome_srv_ = mav->create_service("~/goHome", + std::bind(&MAVManagerServices::goHome_cb, this, _1, _2)); + goTo_srv_ = + mav->create_service("~/goTo", std::bind(&MAVManagerServices::goTo_cb, this, _1, _2)); + goToTimed_srv_ = mav->create_service( + "~/goToTimed", std::bind(&MAVManagerServices::goToTimed_cb, this, _1, _2)); + goToRelative_srv_ = mav->create_service( + "~/goToRelative", std::bind(&MAVManagerServices::goToRelative_cb, this, _1, _2)); + setDesVelInWorldFrame_srv_ = mav->create_service( + "~/setDesVelInWorldFrame", std::bind(&MAVManagerServices::setDesVelInWorldFrame_cb, this, _1, _2)); + setDesVelInBodyFrame_srv_ = mav->create_service( + "~/setDesVelInBodyFrame", std::bind(&MAVManagerServices::setDesVelInBodyFrame_cb, this, _1, _2)); + circle_srv_ = mav->create_service( + "~/circle", std::bind(&MAVManagerServices::circle_cb, this, _1, _2)); + lissajous_srv_ = mav->create_service( + "~/lissajous", std::bind(&MAVManagerServices::lissajous_cb, this, _1, _2)); + compound_lissajous_srv_ = mav->create_service( + "~/compound_lissajous", std::bind(&MAVManagerServices::compound_lissajous_cb, this, _1, _2)); + hover_srv_ = + mav->create_service("~/hover", std::bind(&MAVManagerServices::hover_cb, this, _1, _2)); + ehover_srv_ = mav->create_service("~/ehover", + std::bind(&MAVManagerServices::ehover_cb, this, _1, _2)); + land_srv_ = + mav->create_service("~/land", std::bind(&MAVManagerServices::land_cb, this, _1, _2)); + eland_srv_ = + mav->create_service("~/eland", std::bind(&MAVManagerServices::eland_cb, this, _1, _2)); + estop_srv_ = + mav->create_service("~/estop", std::bind(&MAVManagerServices::estop_cb, this, _1, _2)); + } + + protected: + // Making a MAV pointer + std::shared_ptr mav; + + std::string last_cb_; + + // Creating Servers + rclcpp::Service::SharedPtr motors_srv_; + rclcpp::Service::SharedPtr takeoff_srv_; + rclcpp::Service::SharedPtr goHome_srv_; + rclcpp::Service::SharedPtr goTo_srv_; + rclcpp::Service::SharedPtr goToTimed_srv_; + rclcpp::Service::SharedPtr goToRelative_srv_; + rclcpp::Service::SharedPtr setDesVelInWorldFrame_srv_; + rclcpp::Service::SharedPtr setDesVelInBodyFrame_srv_; + rclcpp::Service::SharedPtr circle_srv_; + rclcpp::Service::SharedPtr lissajous_srv_; + rclcpp::Service::SharedPtr compound_lissajous_srv_; + rclcpp::Service::SharedPtr hover_srv_; + rclcpp::Service::SharedPtr ehover_srv_; + rclcpp::Service::SharedPtr land_srv_; + rclcpp::Service::SharedPtr eland_srv_; + rclcpp::Service::SharedPtr estop_srv_; +}; +} // namespace kr_mav_manager diff --git a/kr_mav_manager/package.xml b/kr_mav_manager/package.xml index d9208867..47f0bc80 100644 --- a/kr_mav_manager/package.xml +++ b/kr_mav_manager/package.xml @@ -1,26 +1,36 @@ - + kr_mav_manager - 1.0.0 + 2.0.0 This package provides tools to interface with the kr_mav_control stack Justin Thomas + Pranav Shah BSD - catkin + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages - message_generation - - roscpp - geometry_msgs + rclcpp + builtin_interfaces + rclcpp_action + Eigen3 nav_msgs - std_msgs sensor_msgs + std_msgs + std_srvs kr_mav_msgs kr_tracker_msgs - std_srvs - actionlib + tf2 + tf2_geometry_msgs - message_runtime + ament_lint_auto + ament_lint_common + + ament_cmake + diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 1b3cf218..303070f9 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -1,22 +1,15 @@ // kr_mav_manager -#include +#include // Standard C++ #include -#include +#include -// ROS Related -#include -#include -#include -#include -#include -#include - -// kr_mav_control -#include -#include +// ROS2 related +// equivalent to #include +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace kr_mav_manager { @@ -29,15 +22,16 @@ static const std::string circle_tracker_str("kr_trackers/CircleTracker"); static const std::string lissajous_tracker_str("kr_trackers/LissajousTracker"); static const std::string lissajous_adder_str("kr_trackers/LissajousAdder"); -MAVManager::MAVManager(std::string ns) - : nh_(ns), - priv_nh_("~"), +using namespace std::placeholders; + +MAVManager::MAVManager() + : Node("manager"), active_tracker_(""), status_(INIT), - last_odom_t_(0.0), - last_imu_t_(0.0), - last_output_data_t_(0.0), - last_heartbeat_t_(0.0), + last_odom_t_(0, 0), + last_imu_t_(0, 0), + last_output_data_t_(0, 0), + last_heartbeat_t_(0, 0), mass_(-1.0), odom_q_(1.0, 0.0, 0.0, 0.0), imu_q_(1.0, 0.0, 0.0, 0.0), @@ -45,128 +39,172 @@ MAVManager::MAVManager(std::string ns) need_imu_(false), need_output_data_(true), need_odom_(true), - use_attitude_safety_catch_(true), - line_tracker_distance_client_(nh_, "trackers_manager/line_tracker_distance/LineTracker", true), - line_tracker_min_jerk_client_(nh_, "trackers_manager/line_tracker_min_jerk/LineTracker", true), - circle_tracker_client_(nh_, "trackers_manager/circle_tracker/CircleTracker", true), - lissajous_tracker_client_(nh_, "trackers_manager/lissajous_tracker/LissajousTracker", true), - lissajous_adder_client_(nh_, "trackers_manager/lissajous_adder/LissajousAdder", true) + use_attitude_safety_catch_(true) { - // Action servers. - float server_wait_timeout; - priv_nh_.param("server_wait_timeout", server_wait_timeout, 0.5f); + // Declaring parameters + this->declare_parameter("server_wait_timeout", 0.5f); + this->declare_parameter("need_imu", rclcpp::PARAMETER_BOOL); + this->declare_parameter("need_output_data", rclcpp::PARAMETER_BOOL); + this->declare_parameter("use_attitude_safety_catch", rclcpp::PARAMETER_BOOL); + this->declare_parameter("max_attitude_angle", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("mass", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("odom_timeout", 0.1f); + this->declare_parameter("takeoff_height", 0.1); + + // Action Client + line_tracker_distance_client_ = + rclcpp_action::create_client(this, "trackers_manager/line_tracker_distance/LineTracker"); + line_tracker_min_jerk_client_ = + rclcpp_action::create_client(this, "trackers_manager/line_tracker_min_jerk/LineTracker"); + circle_tracker_client_ = + rclcpp_action::create_client(this, "trackers_manager/circle_tracker/CircleTracker"); + lissajous_tracker_client_ = + rclcpp_action::create_client(this, "trackers_manager/lissajous_tracker/LissajousTracker"); + lissajous_adder_client_ = + rclcpp_action::create_client(this, "trackers_manager/lissajous_adder/LissajousAdder"); + + float server_wait_timout; + server_wait_timout = this->get_parameter("server_wait_timeout").as_double(); + auto server_wait_timeout_duration = std::chrono::duration(server_wait_timout); - if(!line_tracker_distance_client_.waitForServer(ros::Duration(server_wait_timeout))) + if(!line_tracker_distance_client_->wait_for_action_server(server_wait_timeout_duration)) { - ROS_ERROR("LineTrackerDistance server not found."); + RCLCPP_ERROR(this->get_logger(), "LineTrackerDistance server not found."); } - if(!line_tracker_min_jerk_client_.waitForServer(ros::Duration(server_wait_timeout))) + if(!line_tracker_min_jerk_client_->wait_for_action_server(server_wait_timeout_duration)) { - ROS_ERROR("LineTrackerMinJerk server not found."); + RCLCPP_ERROR(this->get_logger(), "LineTrackerMinJerk server not found."); } - // Optional trackers. - if(!circle_tracker_client_.waitForServer(ros::Duration(server_wait_timeout))) + // Optional Trackers + if(!circle_tracker_client_->wait_for_action_server(server_wait_timeout_duration)) { - ROS_WARN("CircleTracker server not found."); + RCLCPP_WARN(this->get_logger(), "CircleTracker server not found."); } - if(!lissajous_tracker_client_.waitForServer(ros::Duration(server_wait_timeout))) + if(!lissajous_tracker_client_->wait_for_action_server(server_wait_timeout_duration)) { - ROS_WARN("LissajousTracker server not found."); + RCLCPP_WARN(this->get_logger(), "LissajousTracker server not found."); } - if(!lissajous_adder_client_.waitForServer(ros::Duration(server_wait_timeout))) + if(!lissajous_adder_client_->wait_for_action_server(server_wait_timeout_duration)) { - ROS_WARN("LissajousAdder server not found."); + RCLCPP_WARN(this->get_logger(), "LissajousAdder server not found."); } - pub_motors_ = nh_.advertise("motors", 10); - pub_estop_ = nh_.advertise("estop", 10); - pub_so3_command_ = nh_.advertise("so3_cmd", 10); - pub_trpy_command_ = nh_.advertise("trpy_cmd", 10); - pub_position_command_ = nh_.advertise("position_cmd", 10); - pub_status_ = priv_nh_.advertise("status", 10); - pub_goal_velocity_ = nh_.advertise("trackers_manager/velocity_tracker/goal", 10); + // Publishers + pub_motors_ = this->create_publisher("motors", 10); + pub_estop_ = this->create_publisher("estop", 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("position_cmd", 10); + pub_status_ = this->create_publisher("~/status", 10); + pub_goal_velocity_ = + this->create_publisher("trackers_manager/velocity_tracker/goal", 10); - // pwm_command_pub_ = nh_ ... + // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() + 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_ = nh_.subscribe("odom", 10, &MAVManager::odometry_cb, this, ros::TransportHints().tcpNoDelay()); - heartbeat_sub_ = nh_.subscribe("heartbeat", 10, &MAVManager::heartbeat_cb, this, ros::TransportHints().tcpNoDelay()); - tracker_status_sub_ = nh_.subscribe("trackers_manager/status", 10, &MAVManager::tracker_status_cb, this, - ros::TransportHints().tcpNoDelay()); + odom_sub_ = + this->create_subscription("odom", qos, std::bind(&MAVManager::odometry_cb, this, _1)); + heartbeat_sub_ = + this->create_subscription("heartbeat", qos, std::bind(&MAVManager::heartbeat_cb, this, _1)); + tracker_status_sub_ = this->create_subscription( + "trackers_manager/status", qos, std::bind(&MAVManager::tracker_status_cb, this, _1)); // Services - srv_transition_ = nh_.serviceClient("trackers_manager/transition"); + srv_transition_ = this->create_client("trackers_manager/transition"); - srv_transition_.waitForExistence(); + srv_transition_->wait_for_service(); if(!this->transition(null_tracker_str)) - ROS_FATAL("Activation of NullTracker failed."); + { + RCLCPP_FATAL(this->get_logger(), "Activation of NullTracker failed."); + } // Load params after we are sure that we have stuff loaded - if(!priv_nh_.getParam("need_imu", need_imu_)) - ROS_WARN("Couldn't find need_imu param"); + if(!this->get_parameter("need_imu", need_imu_)) + { + RCLCPP_WARN(this->get_logger(), "Couldn't find need_imu param"); + } if(need_imu_) - imu_sub_ = nh_.subscribe("quad_decode_msg/imu", 10, &MAVManager::imu_cb, this); + { + imu_sub_ = this->create_subscription("quad_decode_msg/imu", 10, + std::bind(&MAVManager::imu_cb, this, _1)); + } - if(!priv_nh_.getParam("need_output_data", need_output_data_)) - ROS_WARN("Couldn't find need_output_data param"); + if(!this->get_parameter("need_output_data", need_output_data_)) + { + RCLCPP_WARN(this->get_logger(), "Couldn't find need_output_data param"); + } if(need_output_data_) - output_data_sub_ = nh_.subscribe("quad_decode_msg/output_data", 10, &MAVManager::output_data_cb, this); - - if(!priv_nh_.getParam("use_attitude_safety_catch", use_attitude_safety_catch_)) - ROS_WARN("Couldn't find use_attitude_safety_catch param"); + { + output_data_sub_ = this->create_subscription( + "quad_decode_msg/output_data", 10, std::bind(&MAVManager::output_data_cb, this, _1)); + } - if(!priv_nh_.getParam("max_attitude_angle", max_attitude_angle_)) - ROS_WARN("Couldn't find max_attitude_angle param"); + if(!this->get_parameter("use_attitide_safety_catch", use_attitude_safety_catch_)) + { + RCLCPP_WARN(this->get_logger(), "Couldn't find use_attitude_safety_catch param"); + } double m; - if(!nh_.getParam("mass", m)) - ROS_ERROR("MAV Manager requires mass to be set as a param."); + if(!this->get_parameter("mass", m)) + { + RCLCPP_ERROR(this->get_logger(), "MAV Manager requires mass to be set as a param."); + } else if(this->set_mass(m)) - ROS_INFO("MAVManager using mass = %2.2f.", mass_); + { + RCLCPP_INFO(this->get_logger(), "MAV Manager using mass = %2.2f.", mass_); + } else - ROS_ERROR("Mass failed to set. Perhaps mass <= 0?"); + { + RCLCPP_ERROR(this->get_logger(), "Mass failed to set. Perhaps mass <= 0?"); + } - priv_nh_.param("odom_timeout", odom_timeout_, 0.1f); + this->get_parameter("odom_timeout", odom_timeout_); - // Disable motors if(!this->set_motors(false)) - ROS_ERROR("Could not disable motors"); + { + RCLCPP_ERROR(this->get_logger(), "Could not disable motors"); + } } -void MAVManager::tracker_done_callback(const actionlib::SimpleClientGoalState &state, - const kr_tracker_msgs::LineTrackerResultConstPtr &result) +void MAVManager::tracker_done_callback(const LineTrackerGoalHandle::WrappedResult &result) { - ROS_INFO("Goal (%2.2f, %2.2f, %2.2f, %2.2f) finished with state %s after %2.2f s. and %2.2f m.", result->x, result->y, - result->z, result->yaw, state.toString().c_str(), result->duration, result->length); + auto status = result.code; + RCLCPP_INFO(this->get_logger(), + "Goal (%2.2f, %2.2f, %2.2f, %2.2f) finished with state %s after %2.2f s. and %2.2f m.", result.result->x, + result.result->y, result.result->z, result.result->yaw, result_status[status].c_str(), + result.result->duration, result.result->length); } -void MAVManager::circle_tracker_done_callback(const actionlib::SimpleClientGoalState &state, - const kr_tracker_msgs::CircleTrackerResultConstPtr &result) +void MAVManager::circle_tracker_done_callback(const CircleTrackerGoalHandle::WrappedResult &result) { - ROS_INFO("Circle tracking completed after %2.2f seconds.", result->duration); + RCLCPP_INFO(this->get_logger(), "Circle tracking completed after %2.2f seconds", result.result->duration); } -void MAVManager::lissajous_tracker_done_callback(const actionlib::SimpleClientGoalState &state, - const kr_tracker_msgs::LissajousTrackerResultConstPtr &result) +void MAVManager::lissajous_tracker_done_callback(const LissajousTrackerGoalHandle::WrappedResult &result) { - ROS_INFO("Lissajous tracking completed. Duration: %2.2f seconds, distance: %2.2f m, now located at (%2.2f, %2.2f, " - "%2.2f, %2.2f).", - result->duration, result->length, result->x, result->y, result->z, result->yaw); + RCLCPP_INFO(this->get_logger(), + "Lissajous tracking completed. Duration %2.2f seconds, distance: %2.2f m, now located at (%2.2f, %2.2f, " + "%2.2f, %2.2f).", + result.result->duration, result.result->length, result.result->x, result.result->y, result.result->z, + result.result->yaw); } -void MAVManager::lissajous_adder_done_callback(const actionlib::SimpleClientGoalState &state, - const kr_tracker_msgs::LissajousAdderResultConstPtr &result) +void MAVManager::lissajous_adder_done_callback(const LissajousAdderGoalHandle::WrappedResult &result) { - ROS_INFO("Lissajous tracking completed. Duration: %2.2f seconds, distance: %2.2f m, now located at (%2.2f, %2.2f, " - "%2.2f, %2.2f).", - result->duration, result->length, result->x, result->y, result->z, result->yaw); + RCLCPP_INFO(this->get_logger(), + "Lissajous tracking completed. Duration %2.2f seconds, distance: %2.2f m, now located at (%2.2f, %2.2f, " + "%2.2f, %2.2f).", + result.result->duration, result.result->length, result.result->x, result.result->y, result.result->z, + result.result->yaw); } -void MAVManager::odometry_cb(const nav_msgs::Odometry::ConstPtr &msg) +void MAVManager::odometry_cb(nav_msgs::msg::Odometry::ConstSharedPtr msg) { pos_(0) = msg->pose.pose.position.x; pos_(1) = msg->pose.pose.position.y; @@ -179,10 +217,11 @@ void MAVManager::odometry_cb(const nav_msgs::Odometry::ConstPtr &msg) odom_q_ = Quat(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z); - yaw_ = tf::getYaw(msg->pose.pose.orientation); + yaw_ = tf2::getYaw(msg->pose.pose.orientation); + yaw_dot_ = msg->twist.twist.angular.z; - last_odom_t_ = ros::Time::now(); + last_odom_t_ = this->now(); this->heartbeat(); } @@ -191,44 +230,47 @@ bool MAVManager::takeoff() { if(!this->have_recent_odom()) { - ROS_WARN("Cannot takeoff without odometry."); + RCLCPP_WARN(this->get_logger(), "Cannot takeoff without odometry."); return false; } if(!this->setHome()) + { return false; + } if(!this->motors() || status_ != IDLE) { - ROS_WARN("Cannot takeoff unless motors are idling."); + RCLCPP_WARN(this->get_logger(), "Cannot takeoff motors are idling."); return false; } // Only takeoff if currently under NULL_TRACKER if(active_tracker_.compare(null_tracker_str) != 0) { - ROS_WARN("The Null Tracker must be active before taking off"); + RCLCPP_WARN(this->get_logger(), "The Null Tracker must be active before taking off"); return false; } // Read takeoff height double takeoff_height; - priv_nh_.param("takeoff_height", takeoff_height, 0.1); + takeoff_height = this->get_parameter("takeoff_height").as_double(); takeoff_height_ = takeoff_height; if(takeoff_height_ > 3.0f) { - ROS_ERROR("Takeoff Height is Dangerously High"); + RCLCPP_ERROR(this->get_logger(), "Takeoff Height is Dangerously High"); return false; } - ROS_INFO("Initiating launch sequence..."); + RCLCPP_INFO(this->get_logger(), "Initiating launch sequence..."); - kr_tracker_msgs::LineTrackerGoal goal; + auto goal = LineTracker::Goal(); goal.z = takeoff_height_; goal.relative = true; - line_tracker_distance_client_.sendGoal(goal, boost::bind(&MAVManager::tracker_done_callback, this, _1, _2), - ClientType::SimpleActiveCallback(), ClientType::SimpleFeedbackCallback()); + 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); if(this->transition(line_tracker_distance)) { @@ -243,13 +285,13 @@ bool MAVManager::set_mass(float m) { if(m > 0) { - // TODO: This should update the mass in the controller and everywhere else that is necessary. + // TODO: This should update the mass in the controller and everywhere else that is necessary mass_ = m; return true; } else { - ROS_ERROR("Mass must be > 0"); + RCLCPP_ERROR(this->get_logger(), "Mass must be > 0"); return false; } } @@ -261,11 +303,10 @@ bool MAVManager::setHome() home_ = pos_; home_yaw_ = yaw_; home_set_ = true; - return true; } else - ROS_WARN("Cannot set home unless current pose is known."); + RCLCPP_WARN(this->get_logger(), "Cannot set home unless current pose is known."); return false; } @@ -276,7 +317,7 @@ bool MAVManager::goHome() return this->goTo(home_ + Vec3(0, 0, 0.15), home_yaw_); else { - ROS_WARN("Home not set. Cannot go home."); + RCLCPP_WARN(this->get_logger(), "Home not set. Cannot go home."); return false; } } @@ -285,17 +326,18 @@ bool MAVManager::land() { if(!this->motors() || status_ != FLYING) { - ROS_WARN("Not landing since the robot is not already flying."); + RCLCPP_WARN(this->get_logger(), "Not landing since the robot is not already flying."); return false; } - kr_tracker_msgs::LineTrackerGoal goal; + auto goal = LineTracker::Goal(); goal.x = pos_(0); goal.y = pos_(1); goal.z = home_(2); - std::cout << " landing at " << goal.x << " " << goal.y << " " << goal.z << "\n"; - line_tracker_distance_client_.sendGoal(goal, boost::bind(&MAVManager::tracker_done_callback, this, _1, _2), - ClientType::SimpleActiveCallback(), ClientType::SimpleFeedbackCallback()); + 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); + line_tracker_distance_client_->async_send_goal(goal, options); return this->transition(line_tracker_distance); } @@ -304,11 +346,11 @@ bool MAVManager::goTo(float x, float y, float z, float yaw, float v_des, float a { if(!this->motors() || status_ != FLYING) { - ROS_WARN("The robot must be flying before using the goTo method."); + RCLCPP_WARN(this->get_logger(), "The robot must be flying before using the goTo method."); return false; } - kr_tracker_msgs::LineTrackerGoal goal; + auto goal = LineTracker::Goal(); goal.x = x; goal.y = y; @@ -325,16 +367,18 @@ bool MAVManager::goTo(float x, float y, float z, float yaw, float v_des, float a goal.v_des = v_des; goal.a_des = a_des; - line_tracker_min_jerk_client_.sendGoal(goal, boost::bind(&MAVManager::tracker_done_callback, this, _1, _2), - ClientType::SimpleActiveCallback(), ClientType::SimpleFeedbackCallback()); + 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); return this->transition(line_tracker_min_jerk); } bool MAVManager::goToTimed(float x, float y, float z, float yaw, float v_des, float a_des, bool relative, - ros::Duration duration, ros::Time t_start) + rclcpp::Duration duration, rclcpp::Time t_start) { - kr_tracker_msgs::LineTrackerGoal goal; + auto goal = LineTracker::Goal(); goal.x = x; goal.y = y; @@ -353,10 +397,13 @@ bool MAVManager::goToTimed(float x, float y, float z, float yaw, float v_des, fl goal.v_des = v_des; goal.a_des = a_des; - line_tracker_min_jerk_client_.sendGoal(goal, boost::bind(&MAVManager::tracker_done_callback, this, _1, _2), - ClientType::SimpleActiveCallback(), ClientType::SimpleFeedbackCallback()); - ROS_INFO("Going to {%2.2f, %2.2f, %2.2f, %2.2f}%s with duration %2.2f", x, y, z, yaw, - (relative ? " relative to the current position." : ""), duration.toSec()); + 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); + + RCLCPP_INFO(this->get_logger(), "Going to {%2.2f, %2.2f, %2.2f, %2.2f}%s with duration %2.2f", x, y, z, yaw, + (relative ? " relative to the current position." : ""), duration.seconds()); return this->transition(line_tracker_min_jerk); } @@ -365,14 +412,17 @@ bool MAVManager::goTo(Vec4 xyz_yaw, Vec2 v_and_a_des) { return this->goTo(xyz_yaw(0), xyz_yaw(1), xyz_yaw(2), xyz_yaw(3), v_and_a_des(0), v_and_a_des(1)); } + bool MAVManager::goTo(Vec3 xyz, float yaw, Vec2 v_and_a_des) { return this->goTo(xyz(0), xyz(1), xyz(2), yaw, v_and_a_des(0), v_and_a_des(1)); } + bool MAVManager::goTo(Vec3 xyz, Vec2 v_and_a_des) { return this->goTo(xyz(0), xyz(1), xyz(2), yaw_, v_and_a_des(0), v_and_a_des(1)); } + bool MAVManager::goToYaw(float yaw) { return this->goTo(pos_(0), pos_(1), pos_(2), yaw); @@ -382,18 +432,20 @@ bool MAVManager::circle(float Ax, float Ay, float T, float duration) { if(!this->motors() || status_ != FLYING) { - ROS_WARN("The robot must be flying before using the circle method."); + RCLCPP_WARN(this->get_logger(), "The robot must be flying before using the circle method."); return false; } - kr_tracker_msgs::CircleTrackerGoal goal; - goal.Ax = Ax; - goal.Ay = Ay; - goal.T = T; + auto goal = CircleTracker::Goal(); + goal.ax = Ax; + goal.ay = Ay; + goal.t = T; goal.duration = duration; - circle_tracker_client_.sendGoal(goal, boost::bind(&MAVManager::circle_tracker_done_callback, this, _1, _2), - CircleClientType::SimpleActiveCallback(), CircleClientType::SimpleFeedbackCallback()); + auto options = rclcpp_action::Client::SendGoalOptions(); + options.result_callback = std::bind(&MAVManager::circle_tracker_done_callback, this, _1); + + circle_tracker_client_->async_send_goal(goal, options); return this->transition(circle_tracker_str); } @@ -404,11 +456,11 @@ bool MAVManager::lissajous(float x_amp, float y_amp, float z_amp, float yaw_amp, { if(!this->motors() || status_ != FLYING) { - ROS_WARN("The robot must be flying to execute a Lissajous."); + RCLCPP_WARN(this->get_logger(), "The robot must be flying to execute Lissajous"); return false; } - kr_tracker_msgs::LissajousTrackerGoal goal; + auto goal = LissajousTracker::Goal(); goal.x_amp = x_amp; goal.y_amp = y_amp; goal.z_amp = z_amp; @@ -421,9 +473,10 @@ bool MAVManager::lissajous(float x_amp, float y_amp, float z_amp, float yaw_amp, goal.num_cycles = num_cycles; goal.ramp_time = ramp_time; - lissajous_tracker_client_.sendGoal(goal, boost::bind(&MAVManager::lissajous_tracker_done_callback, this, _1, _2), - LissajousClientType::SimpleActiveCallback(), - LissajousClientType::SimpleFeedbackCallback()); + auto options = rclcpp_action::Client::SendGoalOptions(); + options.result_callback = std::bind(&MAVManager::lissajous_tracker_done_callback, this, _1); + + lissajous_tracker_client_->async_send_goal(goal, options); return this->transition(lissajous_tracker_str); } @@ -434,11 +487,11 @@ bool MAVManager::compound_lissajous(float x_amp[2], float y_amp[2], float z_amp[ { if(!this->motors() || status_ != FLYING) { - ROS_WARN("The robot must be flying to execute a Lissajous."); + RCLCPP_WARN(this->get_logger(), "The robot must be flying to execute Lissajous."); return false; } - kr_tracker_msgs::LissajousAdderGoal goal; + auto goal = LissajousAdder::Goal(); goal.x_amp[0] = x_amp[0]; goal.x_amp[1] = x_amp[1]; goal.y_amp[0] = y_amp[0]; @@ -462,33 +515,35 @@ bool MAVManager::compound_lissajous(float x_amp[2], float y_amp[2], float z_amp[ goal.ramp_time[0] = ramp_time[0]; goal.ramp_time[1] = ramp_time[1]; - lissajous_adder_client_.sendGoal(goal, boost::bind(&MAVManager::lissajous_adder_done_callback, this, _1, _2), - CompoundLissajousClientType::SimpleActiveCallback(), - CompoundLissajousClientType::SimpleFeedbackCallback()); + auto options = rclcpp_action::Client::SendGoalOptions(); + options.result_callback = std::bind(&MAVManager::lissajous_adder_done_callback, this, _1); + + lissajous_adder_client_->async_send_goal(goal, options); return this->transition(lissajous_adder_str); } -// World Velocity commands -bool MAVManager::setDesVelInWorldFrame(float x, float y, float z, float yaw, bool use_position_feedback) +// World Velocity Commands +bool MAVManager::setDesVelInWorldFrame(float x, float y, float z, float yaw, bool use_positio_feedback) { if(!this->motors() || status_ != FLYING) { - ROS_WARN("The robot must be flying with motors on before setting a desired velocity."); + RCLCPP_WARN(this->get_logger(), "The robot must be flying with motors on before setting desired velocity."); return false; } - kr_tracker_msgs::VelocityGoal goal; - goal.header.stamp = ros::Time::now(); + auto goal = kr_tracker_msgs::msg::VelocityGoal(); + goal.header.stamp = this->now(); goal.vx = x; goal.vy = y; goal.vz = z; goal.vyaw = yaw; - goal.use_position_gains = use_position_feedback; + goal.use_position_gains = use_positio_feedback; - pub_goal_velocity_.publish(goal); + pub_goal_velocity_->publish(goal); - ROS_INFO("Desired World velocity: (%1.4f, %1.4f, %1.4f, %1.4f)", goal.vx, goal.vy, goal.vz, goal.vyaw); + RCLCPP_INFO(this->get_logger(), "Desired World velocity: (%1.4f, %1.4f, %1.4f, %1.4f)", goal.vx, goal.vy, goal.vz, + goal.vyaw); // Since this could be called quite often, // only try to transition if it is not the active tracker. @@ -500,7 +555,7 @@ bool MAVManager::setDesVelInWorldFrame(float x, float y, float z, float yaw, boo return true; } -// Body Velocity commands +// Body Velocity Commands bool MAVManager::setDesVelInBodyFrame(float x, float y, float z, float yaw, bool use_position_feedback) { Vec3 vel(x, y, z); @@ -508,10 +563,10 @@ bool MAVManager::setDesVelInBodyFrame(float x, float y, float z, float yaw, bool return this->setDesVelInWorldFrame(vel(0), vel(1), vel(2), yaw, use_position_feedback); } -bool MAVManager::setPositionCommand(const kr_mav_msgs::PositionCommand &msg) +bool MAVManager::setPositionCommand(const kr_mav_msgs::msg::PositionCommand &msg) { - // TODO: Need to keep publishing a position command if there is no update. - // Otherwise, no so3_command will be published. + // TODO: Need to keep publishing a position command if there is no update + // Otherwise, no so3_command will be published if(this->motors() && status_ != ESTOP) { @@ -523,23 +578,24 @@ bool MAVManager::setPositionCommand(const kr_mav_msgs::PositionCommand &msg) flag = this->transition(null_tracker_str); if(flag) - pub_position_command_.publish(msg); + pub_position_command_->publish(msg); return flag; } else { - ROS_WARN("Refusing to set PositionCommand since motors are off or robot is not flying."); + RCLCPP_WARN(this->get_logger(), "Refusing to set PositionCommand since motors are off or the robot is not flying."); return false; } } -bool MAVManager::setSO3Command(const kr_mav_msgs::SO3Command &msg) +bool MAVManager::setSO3Command(const kr_mav_msgs::msg::SO3Command &msg) { // Note: To enable motors, the motors method must be used if(!this->motors()) { - ROS_WARN("Refusing to publish an SO3Command until motors have been enabled using the motors method."); + RCLCPP_WARN(this->get_logger(), + "Refusing to publish an SO3Command until motors have been enabled using the motors method."); return false; } @@ -550,17 +606,18 @@ bool MAVManager::setSO3Command(const kr_mav_msgs::SO3Command &msg) flag = this->transition(null_tracker_str); if(flag) - pub_so3_command_.publish(msg); + pub_so3_command_->publish(msg); return flag; } -bool MAVManager::setTRPYCommand(const kr_mav_msgs::TRPYCommand &msg) +bool MAVManager::setTRPYCommand(const kr_mav_msgs::msg::TRPYCommand &msg) { // Note: To enable motors, the motors method must be used if(!this->motors()) { - ROS_WARN("Refusing to publish an SO3Command until motors have been enabled using the motors method."); + RCLCPP_WARN(this->get_logger(), + "Refusing to publish an SO3Command until motors have been enabled using the motors method."); return false; } @@ -571,7 +628,7 @@ bool MAVManager::setTRPYCommand(const kr_mav_msgs::TRPYCommand &msg) flag = this->transition(null_tracker_str); if(flag) - pub_trpy_command_.publish(msg); + pub_trpy_command_->publish(msg); return flag; } @@ -596,39 +653,40 @@ bool MAVManager::set_motors(bool motors) // off, continue anyway. if(motors && !null_tkr) { - ROS_WARN("Could not transition to null_tracker before starting motors"); + RCLCPP_WARN(this->get_logger(), "Could not transition to null_tracker before starting motors"); return false; } - // Enable/Disable motors + // Enable/Diable motors if(motors) - ROS_INFO("Motors starting..."); + RCLCPP_INFO(this->get_logger(), "Motors starting..."); else - ROS_INFO("Motors stopping..."); + RCLCPP_INFO(this->get_logger(), "Motors stopping..."); - std_msgs::Bool motors_cmd; + auto motors_cmd = std_msgs::msg::Bool(); motors_cmd.data = motors; - pub_motors_.publish(motors_cmd); + pub_motors_->publish(motors_cmd); // Publish a couple so3_commands to ensure motors are or are not spinning - kr_mav_msgs::SO3Command so3_cmd; - so3_cmd.header.stamp = ros::Time::now(); + auto so3_cmd = kr_mav_msgs::msg::SO3Command(); + so3_cmd.header.stamp = this->now(); so3_cmd.force.z = FLT_MIN; so3_cmd.orientation.w = 1.0; so3_cmd.aux.enable_motors = motors; - kr_mav_msgs::TRPYCommand trpy_cmd; + auto trpy_cmd = kr_mav_msgs::msg::TRPYCommand(); trpy_cmd.thrust = FLT_MIN; trpy_cmd.aux.enable_motors = motors; // Queue a few to make sure the signal gets through. // Also, the crazyflie interface throttles commands to 30 Hz, so this needs - // to have a sufficent duration. + // to have sufficient duration. + rclcpp::Rate loop_rate(100.0); for(int i = 0; i < 10; i++) { - pub_so3_command_.publish(so3_cmd); - pub_trpy_command_.publish(trpy_cmd); - ros::Duration(1.0 / 100.0).sleep(); + pub_so3_command_->publish(so3_cmd); + pub_trpy_command_->publish(trpy_cmd); + loop_rate.sleep(); } motors_ = motors; @@ -636,19 +694,19 @@ bool MAVManager::set_motors(bool motors) return true; } -void MAVManager::imu_cb(const sensor_msgs::Imu::ConstPtr &msg) +void MAVManager::imu_cb(sensor_msgs::msg::Imu::ConstSharedPtr msg) { - last_imu_t_ = ros::Time::now(); + last_imu_t_ = this->now(); imu_q_ = Quat(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z); this->heartbeat(); } -void MAVManager::output_data_cb(const kr_mav_msgs::OutputData::ConstPtr &msg) +void MAVManager::output_data_cb(kr_mav_msgs::msg::OutputData::ConstSharedPtr msg) { - last_output_data_t_ = ros::Time::now(); - last_imu_t_ = ros::Time::now(); + last_output_data_t_ = this->now(); + last_imu_t_ = this->now(); imu_q_ = Quat(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z); @@ -664,13 +722,14 @@ void MAVManager::output_data_cb(const kr_mav_msgs::OutputData::ConstPtr &msg) this->heartbeat(); } -void MAVManager::tracker_status_cb(const kr_tracker_msgs::TrackerStatus::ConstPtr &msg) +void MAVManager::tracker_status_cb(kr_tracker_msgs::msg::TrackerStatus::ConstSharedPtr msg) { active_tracker_ = msg->tracker; } -void MAVManager::heartbeat_cb(const std_msgs::Empty::ConstPtr &msg) +void MAVManager::heartbeat_cb(std_msgs::msg::Empty::ConstSharedPtr msg) { + (void)msg; this->heartbeat(); } @@ -680,29 +739,29 @@ void MAVManager::heartbeat() const float freq = 10; // Hz // Only need to do monitoring at the specified frequency - ros::Time t = ros::Time::now(); - float dt = (t - last_heartbeat_t_).toSec(); + rclcpp::Time t = this->now(); + float dt = (t - last_heartbeat_t_).seconds(); if(dt < 1 / freq) return; else last_heartbeat_t_ = t; // Publish the status - std_msgs::UInt8 status_msg; + auto status_msg = std_msgs::msg::UInt8(); status_msg.data = status_; - pub_status_.publish(status_msg); + pub_status_->publish(status_msg); // Checking for odom if(this->motors() && need_odom_ && !this->have_recent_odom()) { - ROS_WARN("No recent odometry!"); + RCLCPP_WARN(this->get_logger(), "No recent odometry!"); this->eland(); } // Checking for imu if(this->motors() && need_imu_ && !this->have_recent_imu()) { - ROS_WARN("No recent imu!"); + RCLCPP_WARN(this->get_logger(), "No recent imu!"); this->eland(); } @@ -712,22 +771,22 @@ 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? - // Convert quaterions to tf so we can compute Euler angles, etc. - tf::Quaternion imu_q(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w()); - tf::Quaternion odom_q(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w()); + // 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()); - // Determine a geodesic angle from hover at the same yaw + // determine a geodesic angle from hover at the same yaw double yaw, pitch, roll; - tf::Matrix3x3 R; + tf2::Matrix3x3 R; // TODO: Don't check mocap odom if we have imu feedback // If we don't have IMU feedback, imu_q_ will be the identity rotation - tf::Matrix3x3(imu_q).getEulerYPR(yaw, pitch, roll); + tf2::Matrix3x3(imu_q).getEulerYPR(yaw, pitch, roll); R.setEulerYPR(0, pitch, roll); float imu_geodesic = std::fabs(std::acos(0.5 * (R[0][0] + R[1][1] + R[2][2] - 1))); - tf::Matrix3x3(odom_q).getEulerYPR(yaw, pitch, roll); + tf2::Matrix3x3(odom_q).getEulerYPR(yaw, pitch, roll); R.setEulerYPR(0, pitch, roll); float odom_geodesic = std::fabs(std::acos(0.5 * (R[0][0] + R[1][1] + R[2][2] - 1))); @@ -743,8 +802,9 @@ void MAVManager::heartbeat() { // Reset the timer so we don't keep calling ehover attitude_limit_timer = 0; - ROS_WARN("Attitude exceeded threshold of %2.2f deg! Geodesic = %2.2f deg. Entering emergency hover.", - max_attitude_angle_ * 180.0f / M_PI, geodesic * 180.0f / M_PI); + RCLCPP_WARN(this->get_logger(), + "Attitude exceeded threshold of %2.2f deg! Geodesic = %2.2f deg. Entering emergency hover.", + max_attitude_angle_ * 180.0f / M_PI, geodesic * 180.f / M_PI); this->ehover(); } } @@ -752,7 +812,10 @@ void MAVManager::heartbeat() if(this->have_recent_output_data()) { if(voltage_ < 10.0f) // Note: Asctec firmware uses 9V - ROS_WARN_THROTTLE(10, "Battery voltage = %2.2f V", voltage_); + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 10000, "Battery voltage = %2.2f V", + voltage_); // time in milliseconds + } } // TODO: Incorporate bounding box constraints. Something along the lines of the following. @@ -783,9 +846,9 @@ bool MAVManager::eland() // left the ground, we don't want them to spin up faster. if(this->motors() && (status_ == FLYING || status_ == ELAND)) { - ROS_WARN("Emergency Land"); + RCLCPP_WARN(this->get_logger(), "Emergency Land"); - kr_mav_msgs::PositionCommand goal; + auto goal = kr_mav_msgs::msg::PositionCommand(); goal.acceleration.z = -0.45f; goal.yaw = yaw_; @@ -803,9 +866,9 @@ bool MAVManager::eland() bool MAVManager::estop() { - ROS_WARN("E-STOP"); - std_msgs::Empty estop_cmd; - pub_estop_.publish(estop_cmd); + RCLCPP_WARN(this->get_logger(), "E-STOP"); + auto estop_cmd = std_msgs::msg::Empty(); + pub_estop_->publish(estop_cmd); // Disarm motors if(this->set_motors(false)) @@ -830,24 +893,24 @@ bool MAVManager::hover() // Acceleration should be opposite the velocity component const Vec3 acc = -dir * a_des; - // acc(3) = - copysign(yaw_a_des, yaw_dot_); + // acc(3) = -copysign(yaw_a_des, yaw_dot_); - // vf = vo + a t -> t = (vf - vo) / a + // vf = vo + at -> t = (vf - vo) / a const float t = v_norm / a_des; // float t_yaw = - yaw_dot_ / yaw_a_des; // xf = xo + vo * t + 1/2 * a * t^2 Vec4 goal(pos_(0) + vel_(0) * t + 0.5f * acc(0) * t * t, pos_(1) + vel_(1) * t + 0.5f * acc(1) * t * t, pos_(2) + vel_(2) * t + 0.5f * acc(2) * t * t, - yaw_); // + yaw_dot_ * t_yaw + 0.5 * yaw_a_des * t_yaw * t_yaw); + yaw_); // + yaw_dot_ * t_yaw + 0.5 * yaw_a_des * t_yaw * t_yaw); Vec2 v_and_a_des(std::sqrt(vel_.dot(vel_)), a_des); - ROS_DEBUG("Coasting to hover..."); + RCLCPP_DEBUG(this->get_logger(), "Coasting to hover..."); return this->goTo(goal, v_and_a_des); } - ROS_DEBUG("Hovering in place..."); + RCLCPP_DEBUG(this->get_logger(), "Hovering in place..."); return this->goTo(pos_(0), pos_(1), pos_(2), yaw_); } @@ -855,16 +918,19 @@ bool MAVManager::ehover() { if(!this->motors() || status_ != FLYING) { - ROS_WARN("Will not call emergency hover unless the robot is already flying."); + RCLCPP_WARN(this->get_logger(), "Will not call emergency hover unless the robot is already flying"); return false; } - kr_tracker_msgs::LineTrackerGoal goal; + auto goal = LineTracker::Goal(); goal.x = pos_(0); goal.y = pos_(1); goal.z = pos_(2); - line_tracker_distance_client_.sendGoal(goal, boost::bind(&MAVManager::tracker_done_callback, this, _1, _2), - ClientType::SimpleActiveCallback(), ClientType::SimpleFeedbackCallback()); + + 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); return this->transition(line_tracker_distance); } @@ -872,14 +938,21 @@ bool MAVManager::ehover() bool MAVManager::transition(const std::string &tracker_str) { // usleep(100000); - kr_tracker_msgs::Transition transition_cmd; - transition_cmd.request.tracker = tracker_str; - if(srv_transition_.call(transition_cmd) && transition_cmd.response.success) + auto transition_cmd = std::make_shared(); + transition_cmd->tracker = tracker_str; + + auto future = srv_transition_->async_send_request(transition_cmd); + + if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) { - active_tracker_ = tracker_str; - ROS_INFO("Current tracker: %s", tracker_str.c_str()); - return true; + auto response = future.get(); + if(response->success) + { + active_tracker_ = tracker_str; + RCLCPP_INFO(this->get_logger(), "Current tracker: %s", tracker_str.c_str()); + return true; + } } return false; @@ -887,16 +960,17 @@ bool MAVManager::transition(const std::string &tracker_str) bool MAVManager::have_recent_odom() { - return (ros::Time::now() - last_odom_t_).toSec() < odom_timeout_; + return (this->now() - last_odom_t_).seconds() < odom_timeout_; } bool MAVManager::have_recent_imu() { - return (ros::Time::now() - last_imu_t_).toSec() < 0.1; + return (this->now() - last_imu_t_).seconds() < 0.1; } bool MAVManager::have_recent_output_data() { - return (ros::Time::now() - last_output_data_t_).toSec() < 0.1; + return (this->now() - last_output_data_t_).seconds() < 0.1; } + } // namespace kr_mav_manager diff --git a/kr_mav_manager/src/mav_services.cpp b/kr_mav_manager/src/mav_services.cpp index 5e36d680..026439c0 100644 --- a/kr_mav_manager/src/mav_services.cpp +++ b/kr_mav_manager/src/mav_services.cpp @@ -1,15 +1,11 @@ -#include +#include "kr_mav_manager/mav_manager_services.hpp" int main(int argc, char **argv) { - ros::init(argc, argv, "manager"); - ros::NodeHandle nh; - + rclcpp::init(argc, argv); auto mav = std::make_shared(); - kr_mav_manager::MAVManagerServices mm_srvs(mav); - - ros::spin(); - + rclcpp::spin(mav); + rclcpp::shutdown(); return 0; -} +} \ No newline at end of file diff --git a/kr_mav_manager/src/sample_sub.cpp b/kr_mav_manager/src/sample_sub.cpp index d87f0163..03223625 100644 --- a/kr_mav_manager/src/sample_sub.cpp +++ b/kr_mav_manager/src/sample_sub.cpp @@ -1,3 +1,5 @@ +// TODO: Convert to ROS2 format + #include #include #include diff --git a/kr_mav_manager/srv/Circle.srv b/kr_mav_manager/srv/Circle.srv index 304951f8..7b4d64a0 100644 --- a/kr_mav_manager/srv/Circle.srv +++ b/kr_mav_manager/srv/Circle.srv @@ -1,7 +1,7 @@ -float32 Ax -float32 Ay -float32 T +float32 ax +float32 ay +float32 t float32 duration --- bool success -string message +string message \ No newline at end of file diff --git a/kr_mav_manager/srv/CompoundLissajous.srv b/kr_mav_manager/srv/CompoundLissajous.srv index 9c27e2c4..01d9e21b 100644 --- a/kr_mav_manager/srv/CompoundLissajous.srv +++ b/kr_mav_manager/srv/CompoundLissajous.srv @@ -11,4 +11,4 @@ float32[2] num_cycles float32[2] ramp_time --- bool success -string message +string message \ No newline at end of file diff --git a/kr_mav_manager/srv/GoalTimed.srv b/kr_mav_manager/srv/GoalTimed.srv index 7500b168..62fc8545 100644 --- a/kr_mav_manager/srv/GoalTimed.srv +++ b/kr_mav_manager/srv/GoalTimed.srv @@ -1,6 +1,6 @@ float32[4] goal -duration duration -time t_start +builtin_interfaces/Duration duration +builtin_interfaces/Time t_start --- bool success -string message +string message \ No newline at end of file diff --git a/kr_mav_manager/srv/Lissajous.srv b/kr_mav_manager/srv/Lissajous.srv index 71b05a1f..30d8ab0f 100644 --- a/kr_mav_manager/srv/Lissajous.srv +++ b/kr_mav_manager/srv/Lissajous.srv @@ -11,4 +11,4 @@ float32 num_cycles float32 ramp_time --- bool success -string message +string message \ No newline at end of file diff --git a/kr_mav_manager/srv/Vec4.srv b/kr_mav_manager/srv/Vec4.srv index 3b78135a..406e29bc 100644 --- a/kr_mav_manager/srv/Vec4.srv +++ b/kr_mav_manager/srv/Vec4.srv @@ -1,4 +1,4 @@ float32[4] goal --- bool success -string message +string message \ No newline at end of file From 426e0e401a5a4006ad1e1559df7c0b3aaee9165a Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Tue, 28 Jan 2025 14:11:06 -0500 Subject: [PATCH 18/64] removed unneccessary AddTwoInts Dev test --- rqt_mav_manager/rqt_mav_manager/__init__.py | 1 - 1 file changed, 1 deletion(-) diff --git a/rqt_mav_manager/rqt_mav_manager/__init__.py b/rqt_mav_manager/rqt_mav_manager/__init__.py index cddef78d..2b26f460 100644 --- a/rqt_mav_manager/rqt_mav_manager/__init__.py +++ b/rqt_mav_manager/rqt_mav_manager/__init__.py @@ -11,7 +11,6 @@ import kr_mav_manager.srv import std_srvs.srv -from example_interfaces.srv import AddTwoInts class MavManagerUi(Plugin): From 7a31c07197251a8ccf0f70659205e86be9f6ffb3 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Tue, 4 Feb 2025 14:34:21 -0500 Subject: [PATCH 19/64] integration init commit --- .../launch/control.launch.py | 100 ++++++++++++++++++ .../launch/test.launch.py | 45 +++++++- .../kr_mav_manager/mav_manager_services.hpp | 1 + kr_mav_manager/src/manager.cpp | 16 +-- kr_mav_manager/src/mav_services.cpp | 3 +- trackers/kr_trackers/CMakeLists.txt | 1 + trackers/kr_trackers/plugins_description.xml | 5 + trackers/kr_trackers/src/null_tracker.cpp | 33 +++--- .../config/trackers_manager.yaml | 2 +- .../src/lifecycle_manager.cpp | 9 +- .../src/trackers_manager.cpp | 5 +- 11 files changed, 190 insertions(+), 30 deletions(-) create mode 100644 interfaces/kr_crazyflie_interface/launch/control.launch.py diff --git a/interfaces/kr_crazyflie_interface/launch/control.launch.py b/interfaces/kr_crazyflie_interface/launch/control.launch.py new file mode 100644 index 00000000..608acaa7 --- /dev/null +++ b/interfaces/kr_crazyflie_interface/launch/control.launch.py @@ -0,0 +1,100 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + + robot_ns = LaunchConfiguration('robot') + odom_topic = LaunchConfiguration('odom') + so3_cmd_topic = LaunchConfiguration('so3_cmd') + + robot_arg = DeclareLaunchArgument( + 'robot', default_value='' + ) + odom_arg = DeclareLaunchArgument( + 'odom', default_value='odom' + ) + so3_cmd_arg = DeclareLaunchArgument( + 'so3_cmd', default_value='so3_cmd' + ) + + # Path to the configuration file + config_file = FindPackageShare('kr_crazyflie_interface').find('kr_crazyflie_interface') + '/config/crazyflie.yaml' + + manager_node = Node( + package="kr_mav_manager", + executable="mav_services", + namespace=LaunchConfiguration('robot'), + name="manager", + output='screen' + ) + + rqt_gui_node = Node( + package="rqt_mav_manager", + executable="rqt_mav_manager", + namespace=LaunchConfiguration('robot'), + name="rqt_mav", + output='screen' + ) + + trackers_component = ComposableNodeContainer( + name="trackers_container", + namespace=LaunchConfiguration('robot'), + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="kr_trackers_manager", + plugin="TrackersManager", + name="kr_trackers_manager", + ), + ComposableNode( + package="kr_trackers_manager", + plugin="TrackersManagerLifecycleManager", + name="kr_trackers_manager_lifecycle", + ), + ComposableNode( + package="kr_mav_controllers", + plugin="SO3ControlComponent", + name="so3_controller", + ), + ], + output='screen' + ) + # EXAMPLE BELOW + # Component configuration + # so3cmd_to_crazyflie_component = ComposableNodeContainer( + # name="so3_container", + # namespace=LaunchConfiguration('robot'), + # package="rclcpp_components", + # executable="component_container", + # composable_node_descriptions=[ + # ComposableNode( + # package="kr_crazyflie_interface", + # plugin="SO3CmdToCrazyflie", + # name="so3cmd_to_crazyflie", + # parameters=[config_file], + # remappings=[ + # ("~/odom", odom_topic), + # ("~/so3_cmd", so3_cmd_topic) + # ] + # ) + # ], + # output='screen' + # ) + # Regular Node (not in container) + + + return LaunchDescription([ + robot_arg, + odom_arg, + so3_cmd_arg, + manager_node, + rqt_gui_node, + trackers_component, + + ]) diff --git a/interfaces/kr_crazyflie_interface/launch/test.launch.py b/interfaces/kr_crazyflie_interface/launch/test.launch.py index fa802026..a07109df 100644 --- a/interfaces/kr_crazyflie_interface/launch/test.launch.py +++ b/interfaces/kr_crazyflie_interface/launch/test.launch.py @@ -5,10 +5,13 @@ from launch_ros.substitutions import FindPackageShare from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +import os +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): - robot_ns = LaunchConfiguration('robot') + ld = LaunchDescription() odom_topic = LaunchConfiguration('odom') so3_cmd_topic = LaunchConfiguration('so3_cmd') @@ -25,6 +28,34 @@ def generate_launch_description(): # Path to the configuration file config_file = FindPackageShare('kr_crazyflie_interface').find('kr_crazyflie_interface') + '/config/crazyflie.yaml' + mav_services_node = Node( + package="kr_mav_manager", + executable="mav_services", + namespace=LaunchConfiguration('robot'), + name="manager", + output='screen' + ) + + rqt_gui_node = Node( + package="rqt_mav_manager", + executable="rqt_mav_manager", + namespace=LaunchConfiguration('robot'), + name="rqt_mav", + output='screen', + ) + + trackers_manager_launch_path = os.path.join( + FindPackageShare('kr_trackers_manager').find('kr_trackers_manager'), + 'launch', + 'example.launch.py' + ) + + # Include the trackers manager launch file + trackers_component = IncludeLaunchDescription( + PythonLaunchDescriptionSource(trackers_manager_launch_path) + ) + + # Component configuration so3cmd_to_crazyflie_component = ComposableNodeContainer( name="so3_container", @@ -45,10 +76,16 @@ def generate_launch_description(): ], output='screen' ) + # Regular Node (not in container) + return LaunchDescription([ robot_arg, - odom_arg, - so3_cmd_arg, - so3cmd_to_crazyflie_component + # odom_arg, + # so3_cmd_arg, + # so3cmd_to_crazyflie_component, + mav_services_node, + # rqt_gui_node, + trackers_component, + ]) 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 9f9e82a3..24c5072f 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 @@ -187,6 +187,7 @@ class MAVManagerServices // Constructor MAVManagerServices(std::shared_ptr m) : mav(m), last_cb_("") { + RCLCPP_INFO(this->mav->get_logger(), "Starting Services..."); motors_srv_ = mav->create_service("~/motors", std::bind(&MAVManagerServices::motors_cb, this, _1, _2)); takeoff_srv_ = mav->create_service( diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 303070f9..356e1b77 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -14,13 +14,13 @@ namespace kr_mav_manager { // Strings -static const std::string line_tracker_distance("kr_trackers/LineTrackerDistance"); -static const std::string line_tracker_min_jerk("kr_trackers/LineTrackerMinJerk"); -static const std::string velocity_tracker_str("kr_trackers/VelocityTracker"); -static const std::string null_tracker_str("kr_trackers/NullTracker"); -static const std::string circle_tracker_str("kr_trackers/CircleTracker"); -static const std::string lissajous_tracker_str("kr_trackers/LissajousTracker"); -static const std::string lissajous_adder_str("kr_trackers/LissajousAdder"); +static const std::string line_tracker_distance("LineTrackerDistance"); +static const std::string line_tracker_min_jerk("LineTrackerMinJerk"); +static const std::string velocity_tracker_str("VelocityTracker"); +static const std::string null_tracker_str("NullTracker"); +static const std::string circle_tracker_str("CircleTracker"); +static const std::string lissajous_tracker_str("LissajousTracker"); +static const std::string lissajous_adder_str("LissajousAdder"); using namespace std::placeholders; @@ -119,7 +119,7 @@ MAVManager::MAVManager() srv_transition_ = this->create_client("trackers_manager/transition"); srv_transition_->wait_for_service(); - if(!this->transition(null_tracker_str)) + if (!this->transition(null_tracker_str)) { RCLCPP_FATAL(this->get_logger(), "Activation of NullTracker failed."); } diff --git a/kr_mav_manager/src/mav_services.cpp b/kr_mav_manager/src/mav_services.cpp index 026439c0..e7ad0822 100644 --- a/kr_mav_manager/src/mav_services.cpp +++ b/kr_mav_manager/src/mav_services.cpp @@ -5,7 +5,8 @@ int main(int argc, char **argv) rclcpp::init(argc, argv); auto mav = std::make_shared(); kr_mav_manager::MAVManagerServices mm_srvs(mav); + rclcpp::spin(mav); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/trackers/kr_trackers/CMakeLists.txt b/trackers/kr_trackers/CMakeLists.txt index 9b60dfb3..e23f11fd 100644 --- a/trackers/kr_trackers/CMakeLists.txt +++ b/trackers/kr_trackers/CMakeLists.txt @@ -45,6 +45,7 @@ set(node_dependencies add_library(${PROJECT_NAME}_plugins SHARED src/line_tracker_distance_server.cpp src/line_tracker_min_jerk_server.cpp + src/null_tracker.cpp src/initial_conditions.cpp) ament_target_dependencies(${PROJECT_NAME}_plugins ${node_dependencies}) diff --git a/trackers/kr_trackers/plugins_description.xml b/trackers/kr_trackers/plugins_description.xml index e020a400..73dab00d 100644 --- a/trackers/kr_trackers/plugins_description.xml +++ b/trackers/kr_trackers/plugins_description.xml @@ -1,6 +1,11 @@ + + This is a tracker which acts as an intermediary between other trackers + + This is a tracker which follows a line from the current position to a given goal position using a trapezoidal trajectory with distance along the trajectory as the parameter instead of time. diff --git a/trackers/kr_trackers/src/null_tracker.cpp b/trackers/kr_trackers/src/null_tracker.cpp index 4ab72b41..a1d2cf62 100644 --- a/trackers/kr_trackers/src/null_tracker.cpp +++ b/trackers/kr_trackers/src/null_tracker.cpp @@ -1,39 +1,44 @@ // TODO: convert to ros2 compatible format -#include -#include -#include +#include "kr_tracker_msgs/msg/tracker_status.hpp" +#include "kr_trackers/Tracker.hpp" +#include "rclcpp/rclcpp.hpp" class NullTracker : public kr_trackers_manager::Tracker { public: - void Initialize(const ros::NodeHandle &nh); - bool Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); + void Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent); + bool Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd); void Deactivate(void); - 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); + uint8_t status(); + protected: + rclcpp::Logger logger_{rclcpp::get_logger("trackers_manager")}; + }; -void NullTracker::Initialize(const ros::NodeHandle &nh) {} +void NullTracker::Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) { + RCLCPP_INFO(logger_, "Initialized NullTracker"); +} -bool NullTracker::Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) +bool NullTracker::Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) { return true; } void NullTracker::Deactivate(void) {} -kr_mav_msgs::PositionCommand::ConstPtr NullTracker::update(const nav_msgs::Odometry::ConstPtr &msg) +kr_mav_msgs::msg::PositionCommand::ConstSharedPtr NullTracker::update(const nav_msgs::msg::Odometry::SharedPtr msg) { // Return a null message (will not publish the position command) - return kr_mav_msgs::PositionCommand::Ptr(); + return std::make_shared(); } -uint8_t NullTracker::status() const +uint8_t NullTracker::status() { - return kr_tracker_msgs::TrackerStatus::SUCCEEDED; + return kr_tracker_msgs::msg::TrackerStatus::SUCCEEDED; } -#include +#include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(NullTracker, 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 40bb0c26..22d73b55 100644 --- a/trackers/kr_trackers_manager/config/trackers_manager.yaml +++ b/trackers/kr_trackers_manager/config/trackers_manager.yaml @@ -1,6 +1,6 @@ trackers_manager: ros__parameters: - trackers: ["LineTrackerDistance", "LineTrackerMinJerk"] + trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk"] line_tracker_distance/default_v_des: 0.5 line_tracker_distance/default_a_des: 0.5 line_tracker_distance/epsilon: 0.1 diff --git a/trackers/kr_trackers_manager/src/lifecycle_manager.cpp b/trackers/kr_trackers_manager/src/lifecycle_manager.cpp index 64a1ab59..d8a3de4d 100644 --- a/trackers/kr_trackers_manager/src/lifecycle_manager.cpp +++ b/trackers/kr_trackers_manager/src/lifecycle_manager.cpp @@ -24,28 +24,35 @@ class TrackersManagerLifecycleManager : public rclcpp_lifecycle::LifecycleNode TrackersManagerLifecycleManager::TrackersManagerLifecycleManager(const rclcpp::NodeOptions &options) : LifecycleNode("lifecycle_manager", rclcpp::NodeOptions(options).use_intra_process_comms(true)) { - this->declare_parameter("node_name", "trackers_manager"); + this->declare_parameter("node_name", "kr_trackers_manager"); node_name = this->get_parameter("node_name").as_string(); service_change_state_name = std::string("/") + node_name + std::string("/change_state"); client_ = this->create_client(service_change_state_name); + RCLCPP_INFO(this->get_logger(), service_change_state_name.c_str()); + client_->wait_for_service(); // Configure node RCLCPP_INFO(this->get_logger(), "Trying to configure node"); + auto transition = lifecycle_msgs::msg::Transition(); transition.id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE; transition.label = std::string("configure"); auto request = std::make_shared(); request->transition = transition; auto future = client_->async_send_request(request); + if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) { auto response = future.get(); if(response->success) RCLCPP_INFO(this->get_logger(), "Node configured successfully"); + } else { + RCLCPP_WARN(this->get_logger(), "Failed to configure Node"); } + // TODO: If node not configured, then don't run activation sequence. // Activate node diff --git a/trackers/kr_trackers_manager/src/trackers_manager.cpp b/trackers/kr_trackers_manager/src/trackers_manager.cpp index bf01078a..b0cd52fc 100644 --- a/trackers/kr_trackers_manager/src/trackers_manager.cpp +++ b/trackers/kr_trackers_manager/src/trackers_manager.cpp @@ -68,9 +68,10 @@ LifecycleCallbackReturn TrackersManager::on_configure(const rclcpp_lifecycle::St { try { + RCLCPP_INFO(this->get_logger(), name.c_str()); auto ptr = tracker_loader_.createSharedInstance(name); ptr->Initialize(weak_node); - std::cout << "Loaded successfully\n"; + RCLCPP_INFO(this->get_logger(), "Loaded successfully"); tracker_map_.insert(std::make_pair(name, ptr)); } catch (const pluginlib::LibraryLoadException& e) @@ -132,6 +133,8 @@ void TrackersManager::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg void TrackersManager::transition_callback(const kr_tracker_msgs::srv::Transition::Request::SharedPtr req, const kr_tracker_msgs::srv::Transition::Response::SharedPtr res) { + RCLCPP_INFO(this->get_logger(), "TRANSITION CALLBACK:"); + RCLCPP_INFO(this->get_logger(), req->tracker.c_str()); const std::map>::iterator it = tracker_map_.find(req->tracker); if(it == tracker_map_.end()) { From 984a0808e9825ca545d79a379db8aff4aec35ce2 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Tue, 4 Feb 2025 15:26:13 -0500 Subject: [PATCH 20/64] cleaned up namespacing --- .../launch/test.launch.py | 6 +++--- .../launch/example.launch.py | 19 +++++++++++++------ 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/interfaces/kr_crazyflie_interface/launch/test.launch.py b/interfaces/kr_crazyflie_interface/launch/test.launch.py index a07109df..7f20b621 100644 --- a/interfaces/kr_crazyflie_interface/launch/test.launch.py +++ b/interfaces/kr_crazyflie_interface/launch/test.launch.py @@ -16,7 +16,7 @@ def generate_launch_description(): so3_cmd_topic = LaunchConfiguration('so3_cmd') robot_arg = DeclareLaunchArgument( - 'robot', default_value='' + 'robot', default_value='quadrotor' ) odom_arg = DeclareLaunchArgument( 'odom', default_value='odom' @@ -32,7 +32,7 @@ def generate_launch_description(): package="kr_mav_manager", executable="mav_services", namespace=LaunchConfiguration('robot'), - name="manager", + name="mav_services", output='screen' ) @@ -85,7 +85,7 @@ def generate_launch_description(): # so3_cmd_arg, # so3cmd_to_crazyflie_component, mav_services_node, - # rqt_gui_node, + rqt_gui_node, trackers_component, ]) diff --git a/trackers/kr_trackers_manager/launch/example.launch.py b/trackers/kr_trackers_manager/launch/example.launch.py index fdfefd2f..23bf2f23 100644 --- a/trackers/kr_trackers_manager/launch/example.launch.py +++ b/trackers/kr_trackers_manager/launch/example.launch.py @@ -6,17 +6,20 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode from launch_ros.substitutions import FindPackageShare +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration def generate_launch_description(): - ld = LaunchDescription() - + robot_arg = DeclareLaunchArgument( + 'robot', default_value='quadrotor' + ) # Path to the configuration file config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml' container = ComposableNodeContainer( name="trackers_manager_container", - namespace="", + namespace=LaunchConfiguration('robot'), package="rclcpp_components", executable="component_container_mt", composable_node_descriptions= [ @@ -24,12 +27,14 @@ def generate_launch_description(): package="kr_trackers_manager", plugin="TrackersManager", name="trackers_manager", - parameters=[config_file] + parameters=[config_file], + namespace=LaunchConfiguration('robot') ), ComposableNode( package="kr_trackers_manager", plugin="TrackersManagerLifecycleManager", name="lifecycle_manager", + namespace=LaunchConfiguration('robot'), parameters=[ {'node_name': "trackers_manager"} ] @@ -38,6 +43,8 @@ def generate_launch_description(): output='screen' ) - ld.add_action(container) - return ld + return LaunchDescription([ + robot_arg, + container, + ]) From f284cff6fb97a9cd770bd5fb72c1579ac1c97945 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Wed, 5 Feb 2025 18:22:14 -0500 Subject: [PATCH 21/64] namespacing figured out --- .../launch/test.launch.py | 18 +++++-- .../launch/test.launch.yaml | 50 +++++++++++++++++++ kr_mav_manager/src/manager.cpp | 13 +++-- .../launch/example.launch.py | 6 +-- .../src/lifecycle_manager.cpp | 6 ++- 5 files changed, 78 insertions(+), 15 deletions(-) create mode 100644 interfaces/kr_crazyflie_interface/launch/test.launch.yaml diff --git a/interfaces/kr_crazyflie_interface/launch/test.launch.py b/interfaces/kr_crazyflie_interface/launch/test.launch.py index 7f20b621..0b32a664 100644 --- a/interfaces/kr_crazyflie_interface/launch/test.launch.py +++ b/interfaces/kr_crazyflie_interface/launch/test.launch.py @@ -8,6 +8,8 @@ import os from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import GroupAction +from launch_ros.actions import PushRosNamespace def generate_launch_description(): @@ -49,12 +51,18 @@ def generate_launch_description(): 'launch', 'example.launch.py' ) - + # Include the trackers manager launch file trackers_component = IncludeLaunchDescription( - PythonLaunchDescriptionSource(trackers_manager_launch_path) + PythonLaunchDescriptionSource(trackers_manager_launch_path), ) + trackers_component_with_namespace = GroupAction( + actions=[ + PushRosNamespace(LaunchConfiguration('robot')), + trackers_component, + ] + ) # Component configuration so3cmd_to_crazyflie_component = ComposableNodeContainer( @@ -84,8 +92,8 @@ def generate_launch_description(): # odom_arg, # so3_cmd_arg, # so3cmd_to_crazyflie_component, - mav_services_node, - rqt_gui_node, - trackers_component, + # mav_services_node, + # rqt_gui_node, + trackers_component_with_namespace, ]) diff --git a/interfaces/kr_crazyflie_interface/launch/test.launch.yaml b/interfaces/kr_crazyflie_interface/launch/test.launch.yaml new file mode 100644 index 00000000..006d2c11 --- /dev/null +++ b/interfaces/kr_crazyflie_interface/launch/test.launch.yaml @@ -0,0 +1,50 @@ +launch: + - declare_launch_argument: + name: robot + default_value: quadrotor + + - declare_launch_argument: + name: odom + default_value: odom + + - declare_launch_argument: + name: so3_cmd + default_value: so3_cmd + + - node: + package: rclcpp_components + exec: component_container_mt + name: trackers_manager_container + namespace: $(var robot) # Equivalent to LaunchConfiguration('robot') + output: screen + + - composable_node_container: + name: trackers_manager_container + composable_nodes: + - package: kr_trackers_manager + plugin: TrackersManager + name: trackers_manager + parameters: + - $(var config_file) # Assuming 'config_file' is passed as a launch argument + + - package: kr_trackers_manager + plugin: TrackersManagerLifecycleManager + name: lifecycle_manager + parameters: + - node_name: trackers_manager + + - node: + pkg: kr_mav_manager + exec: mav_services + namespace: $(var robot) + name: mav_services + output: screen + + - node: + pkg: rqt_mav_manager + exec: rqt_mav_manager + namespace: $(var robot) + name: rqt_mav + output: screen + + diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 356e1b77..bfd1a4ca 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -50,18 +50,21 @@ MAVManager::MAVManager() this->declare_parameter("mass", rclcpp::PARAMETER_DOUBLE); this->declare_parameter("odom_timeout", 0.1f); this->declare_parameter("takeoff_height", 0.1); + this->declare_parameter("robot_name", "quadrotor"); + + std::string robot_name = this->get_parameter("robot_name").as_string(); // Action Client line_tracker_distance_client_ = - rclcpp_action::create_client(this, "trackers_manager/line_tracker_distance/LineTracker"); + rclcpp_action::create_client(this, robot_name + "/trackers_manager/line_tracker_distance/LineTracker"); line_tracker_min_jerk_client_ = - rclcpp_action::create_client(this, "trackers_manager/line_tracker_min_jerk/LineTracker"); + rclcpp_action::create_client(this, robot_name +"/trackers_manager/line_tracker_min_jerk/LineTracker"); circle_tracker_client_ = - rclcpp_action::create_client(this, "trackers_manager/circle_tracker/CircleTracker"); + rclcpp_action::create_client(this, robot_name +"/trackers_manager/circle_tracker/CircleTracker"); lissajous_tracker_client_ = - rclcpp_action::create_client(this, "trackers_manager/lissajous_tracker/LissajousTracker"); + rclcpp_action::create_client(this, robot_name +"/trackers_manager/lissajous_tracker/LissajousTracker"); lissajous_adder_client_ = - rclcpp_action::create_client(this, "trackers_manager/lissajous_adder/LissajousAdder"); + rclcpp_action::create_client(this, robot_name +"/trackers_manager/lissajous_adder/LissajousAdder"); float server_wait_timout; server_wait_timout = this->get_parameter("server_wait_timeout").as_double(); diff --git a/trackers/kr_trackers_manager/launch/example.launch.py b/trackers/kr_trackers_manager/launch/example.launch.py index 23bf2f23..b9e2e806 100644 --- a/trackers/kr_trackers_manager/launch/example.launch.py +++ b/trackers/kr_trackers_manager/launch/example.launch.py @@ -12,14 +12,14 @@ def generate_launch_description(): robot_arg = DeclareLaunchArgument( - 'robot', default_value='quadrotor' + 'namespace', default_value='' ) # Path to the configuration file config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml' container = ComposableNodeContainer( name="trackers_manager_container", - namespace=LaunchConfiguration('robot'), + namespace=LaunchConfiguration('namespace'), package="rclcpp_components", executable="component_container_mt", composable_node_descriptions= [ @@ -28,13 +28,11 @@ def generate_launch_description(): plugin="TrackersManager", name="trackers_manager", parameters=[config_file], - namespace=LaunchConfiguration('robot') ), ComposableNode( package="kr_trackers_manager", plugin="TrackersManagerLifecycleManager", name="lifecycle_manager", - namespace=LaunchConfiguration('robot'), parameters=[ {'node_name': "trackers_manager"} ] diff --git a/trackers/kr_trackers_manager/src/lifecycle_manager.cpp b/trackers/kr_trackers_manager/src/lifecycle_manager.cpp index d8a3de4d..296b25a3 100644 --- a/trackers/kr_trackers_manager/src/lifecycle_manager.cpp +++ b/trackers/kr_trackers_manager/src/lifecycle_manager.cpp @@ -15,6 +15,7 @@ class TrackersManagerLifecycleManager : public rclcpp_lifecycle::LifecycleNode private: rclcpp::Client::SharedPtr client_; std::string node_name; + std::string node_namespace; std::string service_change_state_name; }; @@ -25,8 +26,11 @@ TrackersManagerLifecycleManager::TrackersManagerLifecycleManager(const rclcpp::N : LifecycleNode("lifecycle_manager", rclcpp::NodeOptions(options).use_intra_process_comms(true)) { this->declare_parameter("node_name", "kr_trackers_manager"); + + node_namespace = this->get_namespace(); node_name = this->get_parameter("node_name").as_string(); - service_change_state_name = std::string("/") + node_name + std::string("/change_state"); + service_change_state_name = + node_namespace +std::string("/") + node_name + std::string("/change_state"); client_ = this->create_client(service_change_state_name); RCLCPP_INFO(this->get_logger(), service_change_state_name.c_str()); From 1d1deb74acf11b0db1647c2666d0d1f517d72d4f Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Wed, 5 Feb 2025 18:50:37 -0500 Subject: [PATCH 22/64] namespacing cleared,we might be go for hardware/simulator integration --- ' | 105 ++++++++++++++++++ .../launch/test.launch.py | 9 +- .../config/trackers_manager.yaml | 2 +- .../launch/example.launch.py | 13 ++- .../src/lifecycle_manager.cpp | 2 +- .../src/trackers_manager.cpp | 12 +- 6 files changed, 136 insertions(+), 7 deletions(-) create mode 100644 ' diff --git a/' b/' new file mode 100644 index 00000000..6a4d92de --- /dev/null +++ b/' @@ -0,0 +1,105 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +import os +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import GroupAction +from launch_ros.actions import PushRosNamespace + +def generate_launch_description(): + + ld = LaunchDescription() + odom_topic = LaunchConfiguration('odom') + so3_cmd_topic = LaunchConfiguration('so3_cmd') + + robot_arg = DeclareLaunchArgument( + 'robot', default_value='quadrotor' + ) + odom_arg = DeclareLaunchArgument( + 'odom', default_value='odom' + ) + so3_cmd_arg = DeclareLaunchArgument( + 'so3_cmd', default_value='so3_cmd' + ) + + # Path to the configuration file + config_file = FindPackageShare('kr_crazyflie_interface').find('kr_crazyflie_interface') + '/config/crazyflie.yaml' + + mav_services_node = Node( + package="kr_mav_manager", + executable="mav_services", + namespace=LaunchConfiguration('robot'), + name="mav_services", + output='screen' + ) + + rqt_gui_node = Node( + package="rqt_mav_manager", + executable="rqt_mav_manager", + namespace=LaunchConfiguration('robot'), + name="rqt_mav", + output='screen', + ) + + trackers_manager_config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml' + print(trackers_manager_config_file) + + trackers_manager_launch_path = os.path.join( + FindPackageShare('kr_trackers_manager').find('kr_trackers_manager'), + 'launch', + 'example.launch.py' + ) + + # Include the trackers manager launch file + trackers_component = IncludeLaunchDescription( + PythonLaunchDescriptionSource(trackers_manager_launch_path), + launch_arguments={ + 'trackers_manager_config_file': config_file + }.items() + ) + + trackers_component_with_namespace = GroupAction( + actions=[ + PushRosNamespace(LaunchConfiguration('robot')), + trackers_component, + ] + ) + + # Component configuration + so3cmd_to_crazyflie_component = ComposableNodeContainer( + name="so3_container", + namespace=LaunchConfiguration('robot'), + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="kr_crazyflie_interface", + plugin="SO3CmdToCrazyflie", + name="so3cmd_to_crazyflie", + parameters=[config_file], + remappings=[ + ("~/odom", odom_topic), + ("~/so3_cmd", so3_cmd_topic) + ] + ) + ], + output='screen' + ) + # Regular Node (not in container) + + + return LaunchDescription([ + robot_arg, + # odom_arg, + # so3_cmd_arg, + # so3cmd_to_crazyflie_component, + # mav_services_node, + # rqt_gui_node, + trackers_component_with_namespace, + + ]) diff --git a/interfaces/kr_crazyflie_interface/launch/test.launch.py b/interfaces/kr_crazyflie_interface/launch/test.launch.py index 0b32a664..0b05bb40 100644 --- a/interfaces/kr_crazyflie_interface/launch/test.launch.py +++ b/interfaces/kr_crazyflie_interface/launch/test.launch.py @@ -45,6 +45,8 @@ def generate_launch_description(): name="rqt_mav", output='screen', ) + + trackers_manager_config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml' trackers_manager_launch_path = os.path.join( FindPackageShare('kr_trackers_manager').find('kr_trackers_manager'), @@ -55,6 +57,9 @@ def generate_launch_description(): # Include the trackers manager launch file trackers_component = IncludeLaunchDescription( PythonLaunchDescriptionSource(trackers_manager_launch_path), + launch_arguments={ + 'config_file': trackers_manager_config_file + }.items() ) trackers_component_with_namespace = GroupAction( @@ -92,8 +97,8 @@ def generate_launch_description(): # odom_arg, # so3_cmd_arg, # so3cmd_to_crazyflie_component, - # mav_services_node, - # rqt_gui_node, + mav_services_node, + rqt_gui_node, trackers_component_with_namespace, ]) diff --git a/trackers/kr_trackers_manager/config/trackers_manager.yaml b/trackers/kr_trackers_manager/config/trackers_manager.yaml index 22d73b55..a6dfa465 100644 --- a/trackers/kr_trackers_manager/config/trackers_manager.yaml +++ b/trackers/kr_trackers_manager/config/trackers_manager.yaml @@ -1,4 +1,4 @@ -trackers_manager: +quadrotor/trackers_manager: ros__parameters: trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk"] line_tracker_distance/default_v_des: 0.5 diff --git a/trackers/kr_trackers_manager/launch/example.launch.py b/trackers/kr_trackers_manager/launch/example.launch.py index b9e2e806..558e45f7 100644 --- a/trackers/kr_trackers_manager/launch/example.launch.py +++ b/trackers/kr_trackers_manager/launch/example.launch.py @@ -6,16 +6,23 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode from launch_ros.substitutions import FindPackageShare -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction from launch.substitutions import LaunchConfiguration +def print_config_file(context, *args, **kwargs): + # Get the evaluated value of config_file + config_file_value = LaunchConfiguration('config_file').perform(context) + print(f"Config file path: {config_file_value}") # Prints to the console + return [LogInfo(msg=f"Config file path: {config_file_value}")] # Logs via ROS + def generate_launch_description(): robot_arg = DeclareLaunchArgument( 'namespace', default_value='' ) + config_file_arg = DeclareLaunchArgument('config_file') # Path to the configuration file - config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml' + config_file = LaunchConfiguration('config_file') container = ComposableNodeContainer( name="trackers_manager_container", @@ -44,5 +51,7 @@ def generate_launch_description(): return LaunchDescription([ robot_arg, + config_file_arg, + OpaqueFunction(function=print_config_file), # Call the function to print config_file container, ]) diff --git a/trackers/kr_trackers_manager/src/lifecycle_manager.cpp b/trackers/kr_trackers_manager/src/lifecycle_manager.cpp index 296b25a3..c2667dd1 100644 --- a/trackers/kr_trackers_manager/src/lifecycle_manager.cpp +++ b/trackers/kr_trackers_manager/src/lifecycle_manager.cpp @@ -30,7 +30,7 @@ TrackersManagerLifecycleManager::TrackersManagerLifecycleManager(const rclcpp::N node_namespace = this->get_namespace(); node_name = this->get_parameter("node_name").as_string(); service_change_state_name = - node_namespace +std::string("/") + node_name + std::string("/change_state"); + node_namespace + std::string("/") + node_name + std::string("/change_state"); client_ = this->create_client(service_change_state_name); RCLCPP_INFO(this->get_logger(), service_change_state_name.c_str()); diff --git a/trackers/kr_trackers_manager/src/trackers_manager.cpp b/trackers/kr_trackers_manager/src/trackers_manager.cpp index b0cd52fc..d0750498 100644 --- a/trackers/kr_trackers_manager/src/trackers_manager.cpp +++ b/trackers/kr_trackers_manager/src/trackers_manager.cpp @@ -63,12 +63,22 @@ LifecycleCallbackReturn TrackersManager::on_configure(const rclcpp_lifecycle::St this->declare_parameter>("trackers", {"LineTrackerDistance"}); std::vector trackers_array; this->get_parameter("trackers", trackers_array); + auto param_names = this->list_parameters({}, 10).names; + + // Print each parameter name and its value + for (const auto &name : param_names) { + rclcpp::Parameter param; + if (this->get_parameter(name, param)) { + RCLCPP_INFO(this->get_logger(), "Parameter: %s = %s", + name.c_str(), param.value_to_string().c_str()); + } + } for(auto name: trackers_array) { try { - RCLCPP_INFO(this->get_logger(), name.c_str()); + RCLCPP_INFO_STREAM(this->get_logger(), "Adding " << name.c_str() << " to tracker_map_"); auto ptr = tracker_loader_.createSharedInstance(name); ptr->Initialize(weak_node); RCLCPP_INFO(this->get_logger(), "Loaded successfully"); From f297138083348ef5a4d9307a50c5f0af71995082 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Wed, 5 Feb 2025 19:02:25 -0500 Subject: [PATCH 23/64] mav services can find trackers now --- kr_mav_manager/src/manager.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index bfd1a4ca..3041a49b 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -53,18 +53,19 @@ MAVManager::MAVManager() this->declare_parameter("robot_name", "quadrotor"); std::string robot_name = this->get_parameter("robot_name").as_string(); + RCLCPP_INFO(this->get_logger(), robot_name.c_str()); // Action Client line_tracker_distance_client_ = - rclcpp_action::create_client(this, robot_name + "/trackers_manager/line_tracker_distance/LineTracker"); + rclcpp_action::create_client(this, "trackers_manager/line_tracker_distance/LineTracker"); line_tracker_min_jerk_client_ = - rclcpp_action::create_client(this, robot_name +"/trackers_manager/line_tracker_min_jerk/LineTracker"); + rclcpp_action::create_client(this, "trackers_manager/line_tracker_min_jerk/LineTracker"); circle_tracker_client_ = - rclcpp_action::create_client(this, robot_name +"/trackers_manager/circle_tracker/CircleTracker"); + rclcpp_action::create_client(this, "trackers_manager/circle_tracker/CircleTracker"); lissajous_tracker_client_ = - rclcpp_action::create_client(this, robot_name +"/trackers_manager/lissajous_tracker/LissajousTracker"); + rclcpp_action::create_client(this, "trackers_manager/lissajous_tracker/LissajousTracker"); lissajous_adder_client_ = - rclcpp_action::create_client(this, robot_name +"/trackers_manager/lissajous_adder/LissajousAdder"); + rclcpp_action::create_client(this, "trackers_manager/lissajous_adder/LissajousAdder"); float server_wait_timout; server_wait_timout = this->get_parameter("server_wait_timeout").as_double(); From 2567dc8d5721652f027a09311175e8e1c45e07e0 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Wed, 5 Feb 2025 19:14:10 -0500 Subject: [PATCH 24/64] for some reason there is an immediate segfault from motors_on --- kr_mav_manager/include/kr_mav_manager/mav_manager_services.hpp | 1 + 1 file changed, 1 insertion(+) 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 24c5072f..3f53f426 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 @@ -20,6 +20,7 @@ class MAVManagerServices void motors_cb(const std_srvs::srv::SetBool::Request::SharedPtr req, const std_srvs::srv::SetBool::Response::SharedPtr res) { + RCLCPP_INFO(this->mav->get_logger(), "Motors cb started!"); res->success = mav->set_motors(req->data); res->message = "Motors "; res->message += req->data ? "on" : "off"; From dacb4ed3bf433f2218ad80c89965270088c9c523 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Tue, 11 Feb 2025 15:25:42 -0500 Subject: [PATCH 25/64] finally got client and subscriber working together --- .../launch/test.launch.py | 3 +- .../kr_mav_manager/mav_manager_services.hpp | 1 + kr_mav_manager/src/manager.cpp | 40 ++++++++++++++----- kr_mav_manager/src/mav_services.cpp | 6 ++- 4 files changed, 37 insertions(+), 13 deletions(-) diff --git a/interfaces/kr_crazyflie_interface/launch/test.launch.py b/interfaces/kr_crazyflie_interface/launch/test.launch.py index 0b05bb40..516820be 100644 --- a/interfaces/kr_crazyflie_interface/launch/test.launch.py +++ b/interfaces/kr_crazyflie_interface/launch/test.launch.py @@ -35,7 +35,8 @@ def generate_launch_description(): executable="mav_services", namespace=LaunchConfiguration('robot'), name="mav_services", - output='screen' + output='screen', + # prefix=['gdbserver localhost:3000'], ) rqt_gui_node = Node( 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 3f53f426..12b62e4e 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 @@ -22,6 +22,7 @@ class MAVManagerServices { RCLCPP_INFO(this->mav->get_logger(), "Motors cb started!"); res->success = mav->set_motors(req->data); + RCLCPP_INFO(this->mav->get_logger(), "Set motors"); res->message = "Motors "; res->message += req->data ? "on" : "off"; if(res->success) diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 3041a49b..7e43ce44 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -651,7 +651,11 @@ bool MAVManager::set_motors(bool motors) if(motors && this->motors()) return true; + RCLCPP_INFO(this->get_logger(), "in set motors"); + + bool null_tkr = this->transition(null_tracker_str); + RCLCPP_INFO(this->get_logger(), "started transition"); // Make sure null_tracker is active before starting motors. If turning motors // off, continue anyway. @@ -942,22 +946,36 @@ bool MAVManager::ehover() bool MAVManager::transition(const std::string &tracker_str) { // usleep(100000); + RCLCPP_INFO(this->get_logger(), "In transition"); auto transition_cmd = std::make_shared(); transition_cmd->tracker = tracker_str; - auto future = srv_transition_->async_send_request(transition_cmd); + using ServiceResponseFuture = + rclcpp::Client::SharedFuture; - if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) - { - auto response = future.get(); - if(response->success) - { - active_tracker_ = tracker_str; - RCLCPP_INFO(this->get_logger(), "Current tracker: %s", tracker_str.c_str()); - return true; - } - } + auto response_received_callback = [this](ServiceResponseFuture future) { + auto result = future.get(); + RCLCPP_INFO(this->get_logger(), "Service Response: %i", result->success); + RCLCPP_INFO(this->get_logger(), "Service Response: %s", result->message.c_str()); + }; + + auto future = srv_transition_->async_send_request(transition_cmd, response_received_callback); + + // if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) + // { + // RCLCPP_INFO(this->get_logger(), "Inside if"); + // auto response = future.get(); + // if(response->success) + // { + // active_tracker_ = tracker_str; + // RCLCPP_INFO(this->get_logger(), "Current tracker: %s", tracker_str.c_str()); + // return true; + // } + // } else { + // RCLCPP_INFO(this->get_logger(), "Inside else"); + + // } return false; } diff --git a/kr_mav_manager/src/mav_services.cpp b/kr_mav_manager/src/mav_services.cpp index e7ad0822..8afc9c8e 100644 --- a/kr_mav_manager/src/mav_services.cpp +++ b/kr_mav_manager/src/mav_services.cpp @@ -6,7 +6,11 @@ int main(int argc, char **argv) auto mav = std::make_shared(); kr_mav_manager::MAVManagerServices mm_srvs(mav); - rclcpp::spin(mav); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(mav); + executor.spin(); + + rclcpp::spin(mav->get_node_base_interface()); rclcpp::shutdown(); return 0; } From 1ba47402484c746f4dd3ba4c0f25b5e99c09b6b6 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Fri, 14 Feb 2025 10:22:18 -0500 Subject: [PATCH 26/64] changes for debugging --- .../launch/test.launch.py | 6 ++-- kr_mav_manager/src/manager.cpp | 33 +++++++------------ 2 files changed, 14 insertions(+), 25 deletions(-) diff --git a/interfaces/kr_crazyflie_interface/launch/test.launch.py b/interfaces/kr_crazyflie_interface/launch/test.launch.py index 516820be..528f1f53 100644 --- a/interfaces/kr_crazyflie_interface/launch/test.launch.py +++ b/interfaces/kr_crazyflie_interface/launch/test.launch.py @@ -95,9 +95,9 @@ def generate_launch_description(): return LaunchDescription([ robot_arg, - # odom_arg, - # so3_cmd_arg, - # so3cmd_to_crazyflie_component, + odom_arg, + so3_cmd_arg, + so3cmd_to_crazyflie_component, mav_services_node, rqt_gui_node, trackers_component_with_namespace, diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 7e43ce44..a2784438 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -954,30 +954,19 @@ bool MAVManager::transition(const std::string &tracker_str) using ServiceResponseFuture = rclcpp::Client::SharedFuture; - auto response_received_callback = [this](ServiceResponseFuture future) { - auto result = future.get(); - RCLCPP_INFO(this->get_logger(), "Service Response: %i", result->success); - RCLCPP_INFO(this->get_logger(), "Service Response: %s", result->message.c_str()); - }; + auto response_received_callback = [this, tracker_str](ServiceResponseFuture future) { + auto result = future.get(); + RCLCPP_INFO(this->get_logger(), "Service Response: %i", result->success); + RCLCPP_INFO(this->get_logger(), "Service Response: %s", result->message.c_str()); + if (result->success) { + active_tracker_ = tracker_str; + RCLCPP_INFO(this->get_logger(), "Current tracker: %s", tracker_str.c_str()); + return true; + } + return false; + }; auto future = srv_transition_->async_send_request(transition_cmd, response_received_callback); - - // if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) - // { - // RCLCPP_INFO(this->get_logger(), "Inside if"); - // auto response = future.get(); - // if(response->success) - // { - // active_tracker_ = tracker_str; - // RCLCPP_INFO(this->get_logger(), "Current tracker: %s", tracker_str.c_str()); - // return true; - // } - // } else { - // RCLCPP_INFO(this->get_logger(), "Inside else"); - - // } - - return false; } bool MAVManager::have_recent_odom() From a983f53cc77f625c4522fc338519fbc4cd22eb94 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Fri, 21 Feb 2025 22:27:29 -0600 Subject: [PATCH 27/64] fixed synchronous service call in transition function --- .../include/kr_mav_manager/manager.hpp | 3 + kr_mav_manager/src/manager.cpp | 59 ++++++++++++++----- kr_mav_manager/src/mav_services.cpp | 3 +- 3 files changed, 48 insertions(+), 17 deletions(-) diff --git a/kr_mav_manager/include/kr_mav_manager/manager.hpp b/kr_mav_manager/include/kr_mav_manager/manager.hpp index ab2553a4..ed51bbb4 100644 --- a/kr_mav_manager/include/kr_mav_manager/manager.hpp +++ b/kr_mav_manager/include/kr_mav_manager/manager.hpp @@ -136,6 +136,8 @@ class MAVManager : public rclcpp::Node bool estop(); bool transition(const std::string &tracker_str); + bool construction_done = false; + bool send_transition_request(std::shared_ptr transition_cmd); EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -206,6 +208,7 @@ class MAVManager : public rclcpp::Node // Services rclcpp::Client::SharedPtr srv_transition_; + rclcpp::CallbackGroup::SharedPtr cb_group_; // Helper variable std::map result_status = {{rclcpp_action::ResultCode::SUCCEEDED, "SUCCEEDED"}, diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index a2784438..68167b11 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -120,10 +120,12 @@ MAVManager::MAVManager() "trackers_manager/status", qos, std::bind(&MAVManager::tracker_status_cb, this, _1)); // Services - srv_transition_ = this->create_client("trackers_manager/transition"); + cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + srv_transition_ = this->create_client("trackers_manager/transition", + rmw_qos_profile_services_default, cb_group_); srv_transition_->wait_for_service(); - if (!this->transition(null_tracker_str)) + if(!this->transition(null_tracker_str)) { RCLCPP_FATAL(this->get_logger(), "Activation of NullTracker failed."); } @@ -174,6 +176,7 @@ MAVManager::MAVManager() { RCLCPP_ERROR(this->get_logger(), "Could not disable motors"); } + construction_done = true; } void MAVManager::tracker_done_callback(const LineTrackerGoalHandle::WrappedResult &result) @@ -653,7 +656,6 @@ bool MAVManager::set_motors(bool motors) RCLCPP_INFO(this->get_logger(), "in set motors"); - bool null_tkr = this->transition(null_tracker_str); RCLCPP_INFO(this->get_logger(), "started transition"); @@ -945,28 +947,55 @@ bool MAVManager::ehover() bool MAVManager::transition(const std::string &tracker_str) { - // usleep(100000); RCLCPP_INFO(this->get_logger(), "In transition"); auto transition_cmd = std::make_shared(); transition_cmd->tracker = tracker_str; - using ServiceResponseFuture = - rclcpp::Client::SharedFuture; - - auto response_received_callback = [this, tracker_str](ServiceResponseFuture future) { - auto result = future.get(); - RCLCPP_INFO(this->get_logger(), "Service Response: %i", result->success); - RCLCPP_INFO(this->get_logger(), "Service Response: %s", result->message.c_str()); - if (result->success) { + if(!construction_done) + { + auto future = srv_transition_->async_send_request(transition_cmd); + RCLCPP_INFO(this->get_logger(), "Sent Tracker Transition request."); + if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) + { + auto response = future.get(); + if(response->success) + { + active_tracker_ = tracker_str; + RCLCPP_INFO(this->get_logger(), "Current tracker: %s", tracker_str.c_str()); + return true; + } + } + return false; + } + else + { + RCLCPP_INFO(this->get_logger(), "Sent Tracker Transition request"); + auto future = std::async(std::launch::async, &MAVManager::send_transition_request, this, transition_cmd); + future.wait_for(std::chrono::seconds(1)); + if(future.get()) + { active_tracker_ = tracker_str; RCLCPP_INFO(this->get_logger(), "Current tracker: %s", tracker_str.c_str()); return true; } - return false; - }; + } + return false; +} - auto future = srv_transition_->async_send_request(transition_cmd, response_received_callback); +bool MAVManager::send_transition_request(std::shared_ptr transition_cmd) +{ + auto future = srv_transition_->async_send_request(transition_cmd); + try + { + auto response = future.get(); + return response->success; + } + catch(const std::exception &e) + { + RCLCPP_WARN(this->get_logger(), "Transition callback failed"); + } + return false; } bool MAVManager::have_recent_odom() diff --git a/kr_mav_manager/src/mav_services.cpp b/kr_mav_manager/src/mav_services.cpp index 8afc9c8e..a4d43698 100644 --- a/kr_mav_manager/src/mav_services.cpp +++ b/kr_mav_manager/src/mav_services.cpp @@ -6,11 +6,10 @@ int main(int argc, char **argv) auto mav = std::make_shared(); kr_mav_manager::MAVManagerServices mm_srvs(mav); - rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(mav); executor.spin(); - rclcpp::spin(mav->get_node_base_interface()); rclcpp::shutdown(); return 0; } From 77fb8ca97b492c9cc632970b82fde9b4706b1caa Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Tue, 18 Mar 2025 17:31:59 -0400 Subject: [PATCH 28/64] almost got motors spinning, need so3 controller --- .../kr_crazyflie_interface/CMakeLists.txt | 3 +- .../launch/cf_test.launch.py | 243 ++++++++++++++++++ .../src/so3cmd_to_crazyflie_component.cpp | 53 +++- .../kr_mav_manager/mav_manager_services.hpp | 2 +- kr_mav_manager/src/manager.cpp | 36 ++- kr_mav_manager/src/mav_services.cpp | 1 + .../config/trackers_manager.yaml | 2 +- 7 files changed, 328 insertions(+), 12 deletions(-) create mode 100644 interfaces/kr_crazyflie_interface/launch/cf_test.launch.py diff --git a/interfaces/kr_crazyflie_interface/CMakeLists.txt b/interfaces/kr_crazyflie_interface/CMakeLists.txt index 594c419f..ee38e737 100644 --- a/interfaces/kr_crazyflie_interface/CMakeLists.txt +++ b/interfaces/kr_crazyflie_interface/CMakeLists.txt @@ -12,9 +12,10 @@ find_package(geometry_msgs REQUIRED) find_package(kr_mav_msgs REQUIRED) find_package(rclcpp_components REQUIRED) find_package(Eigen3 REQUIRED) +find_package(crazyflie_interfaces) add_library(${PROJECT_NAME} SHARED src/so3cmd_to_crazyflie_component.cpp) -ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_components kr_mav_msgs nav_msgs geometry_msgs) +ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_components kr_mav_msgs nav_msgs geometry_msgs crazyflie_interfaces) target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) rclcpp_components_register_nodes(${PROJECT_NAME} "SO3CmdToCrazyflie") diff --git a/interfaces/kr_crazyflie_interface/launch/cf_test.launch.py b/interfaces/kr_crazyflie_interface/launch/cf_test.launch.py new file mode 100644 index 00000000..2b3d943c --- /dev/null +++ b/interfaces/kr_crazyflie_interface/launch/cf_test.launch.py @@ -0,0 +1,243 @@ + +import os +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, GroupAction, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node, ComposableNodeContainer, PushRosNamespace +from launch_ros.descriptions import ComposableNode +from launch.conditions import LaunchConfigurationEquals, IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.substitutions import FindPackageShare + +def parse_yaml(context): + # Load the crazyflies YAML file + crazyflies_yaml = LaunchConfiguration('crazyflies_yaml_file').perform(context) + with open(crazyflies_yaml, 'r') as file: + crazyflies = yaml.safe_load(file) + + # server params + server_yaml = os.path.join( + get_package_share_directory('crazyflie'), + 'config', + 'server.yaml') + + with open(server_yaml, 'r') as ymlfile: + server_yaml_content = yaml.safe_load(ymlfile) + + server_params = [crazyflies] + [server_yaml_content['/crazyflie_server']['ros__parameters']] + # robot description + urdf = os.path.join( + get_package_share_directory('crazyflie'), + 'urdf', + 'crazyflie_description.urdf') + + with open(urdf, 'r') as f: + robot_desc = f.read() + + server_params[1]['robot_description'] = robot_desc + + + return [ + Node( + package='crazyflie', + executable='crazyflie_server.py', + condition=LaunchConfigurationEquals('backend','cflib'), + name='crazyflie_server', + output='screen', + parameters= server_params, + ), + Node( + package='crazyflie', + executable='crazyflie_server', + condition=LaunchConfigurationEquals('backend','cpp'), + name='crazyflie_server', + output='screen', + parameters= server_params, + prefix=PythonExpression(['"xterm -e gdb -ex run --args" if ', LaunchConfiguration('debug'), ' else ""']), + ), + Node( + package='crazyflie_sim', + executable='crazyflie_server', + condition=LaunchConfigurationEquals('backend','sim'), + name='crazyflie_server', + output='screen', + emulate_tty=True, + parameters= server_params, + )] + +def generate_launch_description(): + # Crazyflie default paths + default_crazyflies_yaml_path = os.path.join( + get_package_share_directory('crazyflie'), + 'config', + 'crazyflies.yaml') + + + default_rviz_config_path = os.path.join( + get_package_share_directory('crazyflie'), + 'config', + 'config.rviz') + + telop_yaml_path = os.path.join( + get_package_share_directory('crazyflie'), + 'config', + 'teleop.yaml') + + # KR interface paths & configurations + config_file = FindPackageShare('kr_crazyflie_interface').find('kr_crazyflie_interface') + '/config/crazyflie.yaml' + trackers_manager_config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml' + trackers_manager_launch_path = os.path.join( + FindPackageShare('kr_trackers_manager').find('kr_trackers_manager'), + 'launch', + 'example.launch.py' + ) + + # Define launch arguments + # Original Crazyflie arguments + crazyflie_args = [ + DeclareLaunchArgument('crazyflies_yaml_file', + default_value=default_crazyflies_yaml_path), + DeclareLaunchArgument('rviz_config_file', + default_value=default_rviz_config_path), + DeclareLaunchArgument('backend', default_value='cpp'), + DeclareLaunchArgument('debug', default_value='False'), + DeclareLaunchArgument('rviz', default_value='False'), + DeclareLaunchArgument('gui', default_value='True'), + DeclareLaunchArgument('teleop', default_value='False'), + DeclareLaunchArgument('mocap', default_value='True'), + DeclareLaunchArgument('teleop_yaml_file', default_value=''), + # Adding mocap vicon specific parameters + DeclareLaunchArgument('mocap_server', default_value='mocap.perch'), + DeclareLaunchArgument('mocap_frame_rate', default_value='100'), + DeclareLaunchArgument('mocap_max_accel', default_value='10.0'), + ] + + # KR interface arguments + kr_args = [ + DeclareLaunchArgument('robot', default_value='cf3'), + DeclareLaunchArgument('odom', default_value='odom'), + DeclareLaunchArgument('so3_cmd', default_value='so3_cmd'), + ] + + # Initialize launch description with all arguments + ld = LaunchDescription(crazyflie_args + kr_args) + + # Add the original Crazyflie nodes + ld.add_action(OpaqueFunction(function=parse_yaml)) + + # Add mocap_vicon node + ld.add_action( + Node( + condition=IfCondition(LaunchConfiguration('mocap')), + package='mocap_vicon', + executable='mocap_vicon_node', + name='vicon', + output='screen', + parameters=[ + {'server_address': LaunchConfiguration('mocap_server')}, + {'frame_rate': LaunchConfiguration('mocap_frame_rate')}, + {'max_accel': LaunchConfiguration('mocap_max_accel')}, + {'publish_tf': False}, + {'publish_pts': False}, + {'fixed_frame_id': 'mocap'}, + # Set to [''] to take in ALL models from Vicon + {'model_list': ['']}, + ], + remappings=[ + # Uncomment and modify the remapping if needed + # ('vicon/model_name/odom', '/model_name/odom'), + ] + ) + ) + + # ld.add_action( + # Node( + # condition=LaunchConfigurationEquals('rviz', 'True'), + # package='rviz2', + # namespace='', + # executable='rviz2', + # name='rviz2', + # arguments=['-d', LaunchConfiguration('rviz_config_file')], + # parameters=[{ + # "use_sim_time": PythonExpression(["'", LaunchConfiguration('backend'), "' == 'sim'"]), + # }] + # ) + # ) + + # ld.add_action( + # Node( + # condition=LaunchConfigurationEquals('gui', 'True'), + # package='crazyflie', + # namespace='', + # executable='gui.py', + # name='gui', + # parameters=[{ + # "use_sim_time": PythonExpression(["'", LaunchConfiguration('backend'), "' == 'sim'"]), + # }] + # ) + # ) + + # Add the KR interface nodes + ld.add_action( + Node( + package="kr_mav_manager", + executable="mav_services", + namespace=LaunchConfiguration('robot'), + name="mav_services", + output='screen', + ) + ) + + ld.add_action( + Node( + package="rqt_mav_manager", + executable="rqt_mav_manager", + namespace=LaunchConfiguration('robot'), + name="rqt_mav", + output='screen', + ) + ) + + # Include the trackers manager launch file with namespace + trackers_component = IncludeLaunchDescription( + PythonLaunchDescriptionSource(trackers_manager_launch_path), + launch_arguments={ + 'config_file': trackers_manager_config_file + }.items() + ) + + trackers_component_with_namespace = GroupAction( + actions=[ + PushRosNamespace(LaunchConfiguration('robot')), + trackers_component, + ] + ) + + ld.add_action(trackers_component_with_namespace) + + # Add the SO3 command to Crazyflie component container + ld.add_action( + ComposableNodeContainer( + name="so3_container", + namespace=LaunchConfiguration('robot'), + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="kr_crazyflie_interface", + plugin="SO3CmdToCrazyflie", + name="so3cmd_to_crazyflie", + parameters=[config_file], + remappings=[ + ("~/odom", LaunchConfiguration('odom')), + ("~/so3_cmd", LaunchConfiguration('so3_cmd')) + ] + ) + ], + output='screen' + ) + ) + + return ld \ No newline at end of file diff --git a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp index 2e585961..6bc3a307 100644 --- a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp +++ b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp @@ -13,6 +13,8 @@ #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" +#include "crazyflie_interfaces/srv/arm.hpp" + // TODO: Remove CLAMP as macro #define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x) @@ -26,10 +28,12 @@ class SO3CmdToCrazyflie : public rclcpp::Node private: void so3_cmd_callback(const kr_mav_msgs::msg::SO3Command::UniquePtr msg); void odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom); + void arm_drone(bool arm); // Added method to arm/disarm the drone - bool odom_set_, so3_cmd_set_; + bool odom_set_, so3_cmd_set_, armed_drone_; Eigen::Quaterniond q_odom_; + rclcpp::Client::SharedPtr arm_client_; rclcpp::Publisher::SharedPtr crazy_fast_cmd_vel_pub_, crazy_cmd_vel_pub_; rclcpp::Subscription::SharedPtr so3_cmd_sub_; @@ -68,6 +72,24 @@ void SO3CmdToCrazyflie::odom_callback(const nav_msgs::msg::Odometry::UniquePtr o } } +void SO3CmdToCrazyflie::arm_drone(bool arm) +{ + auto request = std::make_shared(); + request->arm = arm; + + RCLCPP_INFO(this->get_logger(), "Sending arm request: %s", arm ? "true" : "false"); + + // Send the service request + auto future = arm_client_->async_send_request( + request, + [this](rclcpp::Client::SharedFuture future) { + auto result = future.get(); + // RCLCPP_INFO(this->get_logger(), "Arm service call %s", + // result->success ? "succeeded" : "failed"); + } + ); +} + void SO3CmdToCrazyflie::so3_cmd_callback(kr_mav_msgs::msg::SO3Command::UniquePtr msg) { if(!so3_cmd_set_) @@ -76,6 +98,10 @@ void SO3CmdToCrazyflie::so3_cmd_callback(kr_mav_msgs::msg::SO3Command::UniquePtr // switch on motors if(msg->aux.enable_motors) { + if (!armed_drone_) { + arm_drone(true); + armed_drone_ = true; + } // If the crazyflie motors are timed out, we need to send a zero message in order to get them to start if(motor_status_ < 3) { @@ -102,6 +128,10 @@ void SO3CmdToCrazyflie::so3_cmd_callback(kr_mav_msgs::msg::SO3Command::UniquePtr crazy_cmd_vel_pub_->publish(std::move(motors_vel_cmd)); last_so3_cmd_ = *msg; last_so3_cmd_time_ = msg->header.stamp; + if (armed_drone_) { + arm_drone(false); + armed_drone_ = false; + } return; } @@ -209,12 +239,13 @@ SO3CmdToCrazyflie::SO3CmdToCrazyflie(const rclcpp::NodeOptions &options) odom_set_ = false; so3_cmd_set_ = false; + armed_drone_ = false; motor_status_ = 0; // TODO make sure this is publishing to the right place - crazy_fast_cmd_vel_pub_ = this->create_publisher("~/cmd_vel_fast", 10); + crazy_fast_cmd_vel_pub_ = this->create_publisher("/cf3/cmd_vel", 10); - crazy_cmd_vel_pub_ = this->create_publisher("~/cmd_vel", 10); + crazy_cmd_vel_pub_ = this->create_publisher("/cf3/cmd_vel", 10); // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; @@ -222,10 +253,22 @@ SO3CmdToCrazyflie::SO3CmdToCrazyflie(const rclcpp::NodeOptions &options) auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); so3_cmd_sub_ = this->create_subscription( - "~/so3_cmd", qos2, std::bind(&SO3CmdToCrazyflie::so3_cmd_callback, this, std::placeholders::_1)); + "/quadrotor/so3_cmd", qos2, std::bind(&SO3CmdToCrazyflie::so3_cmd_callback, this, std::placeholders::_1)); odom_sub_ = this->create_subscription( - "~/odom", qos1, std::bind(&SO3CmdToCrazyflie::odom_callback, this, std::placeholders::_1)); + "/quadrotor/odom", qos1, std::bind(&SO3CmdToCrazyflie::odom_callback, this, std::placeholders::_1)); + + arm_client_ = this->create_client("/cf3/arm"); + + while (!arm_client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for arm service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "Waiting for arm service to appear..."); + } + + RCLCPP_INFO(this->get_logger(), "Arm service client initialized"); } #include "rclcpp_components/register_node_macro.hpp" 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 12b62e4e..3d095264 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 @@ -189,7 +189,7 @@ class MAVManagerServices // Constructor MAVManagerServices(std::shared_ptr m) : mav(m), last_cb_("") { - RCLCPP_INFO(this->mav->get_logger(), "Starting Services..."); + RCLCPP_INFO(mav->get_logger(), "Starting Services..."); motors_srv_ = mav->create_service("~/motors", std::bind(&MAVManagerServices::motors_cb, this, _1, _2)); takeoff_srv_ = mav->create_service( diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 68167b11..47a29312 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -11,6 +11,7 @@ #include "tf2/utils.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + namespace kr_mav_manager { // Strings @@ -50,9 +51,9 @@ MAVManager::MAVManager() this->declare_parameter("mass", rclcpp::PARAMETER_DOUBLE); this->declare_parameter("odom_timeout", 0.1f); this->declare_parameter("takeoff_height", 0.1); - this->declare_parameter("robot_name", "quadrotor"); + this->declare_parameter("robot", "cf3"); - std::string robot_name = this->get_parameter("robot_name").as_string(); + std::string robot_name = this->get_parameter("robot").as_string(); RCLCPP_INFO(this->get_logger(), robot_name.c_str()); // Action Client @@ -123,12 +124,14 @@ MAVManager::MAVManager() cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); srv_transition_ = this->create_client("trackers_manager/transition", rmw_qos_profile_services_default, cb_group_); + RCLCPP_INFO(this->get_logger(), "1!!!!!!!!!!!!!!"); srv_transition_->wait_for_service(); if(!this->transition(null_tracker_str)) { RCLCPP_FATAL(this->get_logger(), "Activation of NullTracker failed."); } + RCLCPP_INFO(this->get_logger(), "2!!!!!!!!!!!!!!"); // Load params after we are sure that we have stuff loaded if(!this->get_parameter("need_imu", need_imu_)) @@ -140,6 +143,7 @@ MAVManager::MAVManager() imu_sub_ = this->create_subscription("quad_decode_msg/imu", 10, std::bind(&MAVManager::imu_cb, this, _1)); } + RCLCPP_INFO(this->get_logger(), "3!!!!!!!!!!!!!!"); if(!this->get_parameter("need_output_data", need_output_data_)) { @@ -150,11 +154,13 @@ MAVManager::MAVManager() output_data_sub_ = this->create_subscription( "quad_decode_msg/output_data", 10, std::bind(&MAVManager::output_data_cb, this, _1)); } + RCLCPP_INFO(this->get_logger(), "4!!!!!!!!!!!!!!"); if(!this->get_parameter("use_attitide_safety_catch", use_attitude_safety_catch_)) { RCLCPP_WARN(this->get_logger(), "Couldn't find use_attitude_safety_catch param"); } + RCLCPP_INFO(this->get_logger(), "5!!!!!!!!!!!!!!"); double m; if(!this->get_parameter("mass", m)) @@ -169,6 +175,7 @@ MAVManager::MAVManager() { RCLCPP_ERROR(this->get_logger(), "Mass failed to set. Perhaps mass <= 0?"); } + RCLCPP_INFO(this->get_logger(), "6!!!!!!!!!!!!!!"); this->get_parameter("odom_timeout", odom_timeout_); @@ -177,6 +184,7 @@ MAVManager::MAVManager() RCLCPP_ERROR(this->get_logger(), "Could not disable motors"); } construction_done = true; + RCLCPP_INFO(this->get_logger(), "construction of mav services done!!!!!!!!!!!!!!"); } void MAVManager::tracker_done_callback(const LineTrackerGoalHandle::WrappedResult &result) @@ -955,21 +963,39 @@ bool MAVManager::transition(const std::string &tracker_str) if(!construction_done) { auto future = srv_transition_->async_send_request(transition_cmd); - RCLCPP_INFO(this->get_logger(), "Sent Tracker Transition request."); - if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) + //auto future = std::async(std::launch::async, &MAVManager::send_transition_request, this, transition_cmd); + //future.wait_for(std::chrono::seconds(1)); + //RCLCPP_INFO(this->get_logger(), "Not construction done."); + //RCLCPP_INFO(this->get_logger(), "Sent Tracker Transition request."); + //// future.wait_for(std::chrono::seconds(1)); + //if(future.get()) + //{ + // active_tracker_ = tracker_str; + // RCLCPP_INFO(this->get_logger(), "Current tracker: %s", tracker_str.c_str()); + // RCLCPP_INFO(this->get_logger(), "Returning from Transition"); + // return true; + //} + + + rclcpp::FutureReturnCode frc = rclcpp::spin_until_future_complete(this->get_node_base_interface(), future); + RCLCPP_INFO(this->get_logger(), "Done spinning until future complete"); + if(frc == rclcpp::FutureReturnCode::SUCCESS) { auto response = future.get(); if(response->success) { active_tracker_ = tracker_str; RCLCPP_INFO(this->get_logger(), "Current tracker: %s", tracker_str.c_str()); + RCLCPP_INFO(this->get_logger(), "Returning from Transition"); return true; } } + RCLCPP_INFO(this->get_logger(), "Returning from Transition"); return false; } else { + RCLCPP_INFO(this->get_logger(), "Construction done."); RCLCPP_INFO(this->get_logger(), "Sent Tracker Transition request"); auto future = std::async(std::launch::async, &MAVManager::send_transition_request, this, transition_cmd); future.wait_for(std::chrono::seconds(1)); @@ -977,9 +1003,11 @@ bool MAVManager::transition(const std::string &tracker_str) { active_tracker_ = tracker_str; RCLCPP_INFO(this->get_logger(), "Current tracker: %s", tracker_str.c_str()); + RCLCPP_INFO(this->get_logger(), "Returning from Transition"); return true; } } + RCLCPP_INFO(this->get_logger(), "Returning from Transition"); return false; } diff --git a/kr_mav_manager/src/mav_services.cpp b/kr_mav_manager/src/mav_services.cpp index a4d43698..aed341f6 100644 --- a/kr_mav_manager/src/mav_services.cpp +++ b/kr_mav_manager/src/mav_services.cpp @@ -6,6 +6,7 @@ int main(int argc, char **argv) auto mav = std::make_shared(); kr_mav_manager::MAVManagerServices mm_srvs(mav); + rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(mav); executor.spin(); diff --git a/trackers/kr_trackers_manager/config/trackers_manager.yaml b/trackers/kr_trackers_manager/config/trackers_manager.yaml index a6dfa465..8856c360 100644 --- a/trackers/kr_trackers_manager/config/trackers_manager.yaml +++ b/trackers/kr_trackers_manager/config/trackers_manager.yaml @@ -1,4 +1,4 @@ -quadrotor/trackers_manager: +cf3/trackers_manager: ros__parameters: trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk"] line_tracker_distance/default_v_des: 0.5 From 8a3ec9185387545be7c9c143dc66c4de2d55c253 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Tue, 18 Mar 2025 18:23:12 -0400 Subject: [PATCH 29/64] spinning motors! --- .../src/so3cmd_to_crazyflie_component.cpp | 11 +++++++---- kr_mav_controllers/src/so3_control_component.cpp | 8 ++++---- trackers/kr_trackers_manager/launch/example.launch.py | 7 ++++++- 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp index 6bc3a307..4f4d02ad 100644 --- a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp +++ b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp @@ -95,13 +95,16 @@ void SO3CmdToCrazyflie::so3_cmd_callback(kr_mav_msgs::msg::SO3Command::UniquePtr if(!so3_cmd_set_) so3_cmd_set_ = true; + // switch on motors if(msg->aux.enable_motors) { if (!armed_drone_) { arm_drone(true); armed_drone_ = true; + // std::this_thread::sleep_for(std::chrono::seconds(1)); } + // If the crazyflie motors are timed out, we need to send a zero message in order to get them to start if(motor_status_ < 3) { @@ -243,9 +246,9 @@ SO3CmdToCrazyflie::SO3CmdToCrazyflie(const rclcpp::NodeOptions &options) motor_status_ = 0; // TODO make sure this is publishing to the right place - crazy_fast_cmd_vel_pub_ = this->create_publisher("/cf3/cmd_vel", 10); + crazy_fast_cmd_vel_pub_ = this->create_publisher("/cf3/cmd_vel_legacy", 10); - crazy_cmd_vel_pub_ = this->create_publisher("/cf3/cmd_vel", 10); + crazy_cmd_vel_pub_ = this->create_publisher("/cf3/cmd_vel_legacy", 10); // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; @@ -253,10 +256,10 @@ SO3CmdToCrazyflie::SO3CmdToCrazyflie(const rclcpp::NodeOptions &options) auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); so3_cmd_sub_ = this->create_subscription( - "/quadrotor/so3_cmd", qos2, std::bind(&SO3CmdToCrazyflie::so3_cmd_callback, this, std::placeholders::_1)); + "/cf3/so3_cmd", qos2, std::bind(&SO3CmdToCrazyflie::so3_cmd_callback, this, std::placeholders::_1)); odom_sub_ = this->create_subscription( - "/quadrotor/odom", qos1, std::bind(&SO3CmdToCrazyflie::odom_callback, this, std::placeholders::_1)); + "/cf3/odom", qos1, std::bind(&SO3CmdToCrazyflie::odom_callback, this, std::placeholders::_1)); arm_client_ = this->create_client("/cf3/arm"); diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index 9a80d54b..be3f1dc1 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -636,13 +636,13 @@ SO3ControlComponent::SO3ControlComponent(const rclcpp::NodeOptions &options) auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 2), qos_profile); odom_sub_ = this->create_subscription( - "~/odom", qos1, std::bind(&SO3ControlComponent::odom_callback, this, std::placeholders::_1)); + "/cf3/odom", qos1, std::bind(&SO3ControlComponent::odom_callback, this, std::placeholders::_1)); position_cmd_sub_ = this->create_subscription( - "~/position_cmd", qos1, std::bind(&SO3ControlComponent::position_cmd_callback, this, std::placeholders::_1)); + "/cf3/position_cmd", qos1, std::bind(&SO3ControlComponent::position_cmd_callback, this, std::placeholders::_1)); enable_motors_sub_ = this->create_subscription( - "~/motors", qos2, std::bind(&SO3ControlComponent::enable_motors_callback, this, std::placeholders::_1)); + "/cf3/motors", qos2, std::bind(&SO3ControlComponent::enable_motors_callback, this, std::placeholders::_1)); corrections_sub_ = this->create_subscription( - "~/corrections", qos1, std::bind(&SO3ControlComponent::corrections_callback, this, std::placeholders::_1)); + "/cf3/corrections", qos1, std::bind(&SO3ControlComponent::corrections_callback, this, std::placeholders::_1)); } #include "rclcpp_components/register_node_macro.hpp" diff --git a/trackers/kr_trackers_manager/launch/example.launch.py b/trackers/kr_trackers_manager/launch/example.launch.py index 558e45f7..75466d03 100644 --- a/trackers/kr_trackers_manager/launch/example.launch.py +++ b/trackers/kr_trackers_manager/launch/example.launch.py @@ -43,7 +43,12 @@ def generate_launch_description(): parameters=[ {'node_name': "trackers_manager"} ] - ) + ), + ComposableNode( + package="kr_mav_controllers", + plugin="SO3ControlComponent", + name="so3_controller", + ), ], output='screen' ) From e6b8851e5cf599d61311191ae6cf3d598b9281ce Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Wed, 19 Mar 2025 11:12:29 -0400 Subject: [PATCH 30/64] fixed takeoff clock issue --- .../launch/cf_test.launch.py | 2 +- .../include/kr_mav_manager/manager.hpp | 2 +- kr_mav_manager/src/manager.cpp | 35 +- log3880012195.csv | 526 ++++++++++++++++++ params3679191350.csv | 311 +++++++++++ 5 files changed, 852 insertions(+), 24 deletions(-) create mode 100644 log3880012195.csv create mode 100644 params3679191350.csv diff --git a/interfaces/kr_crazyflie_interface/launch/cf_test.launch.py b/interfaces/kr_crazyflie_interface/launch/cf_test.launch.py index 2b3d943c..ec3949e7 100644 --- a/interfaces/kr_crazyflie_interface/launch/cf_test.launch.py +++ b/interfaces/kr_crazyflie_interface/launch/cf_test.launch.py @@ -143,7 +143,7 @@ def generate_launch_description(): {'publish_pts': False}, {'fixed_frame_id': 'mocap'}, # Set to [''] to take in ALL models from Vicon - {'model_list': ['']}, + {'model_list': ['cf3']}, ], remappings=[ # Uncomment and modify the remapping if needed diff --git a/kr_mav_manager/include/kr_mav_manager/manager.hpp b/kr_mav_manager/include/kr_mav_manager/manager.hpp index ed51bbb4..8ef66de0 100644 --- a/kr_mav_manager/include/kr_mav_manager/manager.hpp +++ b/kr_mav_manager/include/kr_mav_manager/manager.hpp @@ -176,7 +176,7 @@ class MAVManager : public rclcpp::Node Vec3 home_, goal_; float home_yaw_; - bool need_imu_, need_output_data_, need_odom_, use_attitude_safety_catch_; + bool need_imu_, need_output_data_, need_odom_, use_attitude_safety_catch_, last_heartbeat_t_initialized_; bool home_set_, motors_; float voltage_, pressure_height_, pressure_dheight_; std::array magnetic_field_; diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 47a29312..5cee067e 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -40,7 +40,8 @@ MAVManager::MAVManager() need_imu_(false), need_output_data_(true), need_odom_(true), - use_attitude_safety_catch_(true) + use_attitude_safety_catch_(true), + last_heartbeat_t_initialized_(false) { // Declaring parameters this->declare_parameter("server_wait_timeout", 0.5f); @@ -124,14 +125,12 @@ MAVManager::MAVManager() cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); srv_transition_ = this->create_client("trackers_manager/transition", rmw_qos_profile_services_default, cb_group_); - RCLCPP_INFO(this->get_logger(), "1!!!!!!!!!!!!!!"); srv_transition_->wait_for_service(); if(!this->transition(null_tracker_str)) { RCLCPP_FATAL(this->get_logger(), "Activation of NullTracker failed."); } - RCLCPP_INFO(this->get_logger(), "2!!!!!!!!!!!!!!"); // Load params after we are sure that we have stuff loaded if(!this->get_parameter("need_imu", need_imu_)) @@ -143,7 +142,6 @@ MAVManager::MAVManager() imu_sub_ = this->create_subscription("quad_decode_msg/imu", 10, std::bind(&MAVManager::imu_cb, this, _1)); } - RCLCPP_INFO(this->get_logger(), "3!!!!!!!!!!!!!!"); if(!this->get_parameter("need_output_data", need_output_data_)) { @@ -154,13 +152,11 @@ MAVManager::MAVManager() output_data_sub_ = this->create_subscription( "quad_decode_msg/output_data", 10, std::bind(&MAVManager::output_data_cb, this, _1)); } - RCLCPP_INFO(this->get_logger(), "4!!!!!!!!!!!!!!"); if(!this->get_parameter("use_attitide_safety_catch", use_attitude_safety_catch_)) { RCLCPP_WARN(this->get_logger(), "Couldn't find use_attitude_safety_catch param"); } - RCLCPP_INFO(this->get_logger(), "5!!!!!!!!!!!!!!"); double m; if(!this->get_parameter("mass", m)) @@ -175,7 +171,6 @@ MAVManager::MAVManager() { RCLCPP_ERROR(this->get_logger(), "Mass failed to set. Perhaps mass <= 0?"); } - RCLCPP_INFO(this->get_logger(), "6!!!!!!!!!!!!!!"); this->get_parameter("odom_timeout", odom_timeout_); @@ -184,7 +179,6 @@ MAVManager::MAVManager() RCLCPP_ERROR(this->get_logger(), "Could not disable motors"); } construction_done = true; - RCLCPP_INFO(this->get_logger(), "construction of mav services done!!!!!!!!!!!!!!"); } void MAVManager::tracker_done_callback(const LineTrackerGoalHandle::WrappedResult &result) @@ -243,16 +237,20 @@ void MAVManager::odometry_cb(nav_msgs::msg::Odometry::ConstSharedPtr msg) bool MAVManager::takeoff() { + + RCLCPP_INFO(this->get_logger(), "Received TAKEOFF"); if(!this->have_recent_odom()) { RCLCPP_WARN(this->get_logger(), "Cannot takeoff without odometry."); return false; } + RCLCPP_INFO(this->get_logger(), "Received TAKEOFF 1"); if(!this->setHome()) { return false; } + RCLCPP_INFO(this->get_logger(), "Received TAKEOFF 2"); if(!this->motors() || status_ != IDLE) { @@ -754,10 +752,17 @@ void MAVManager::heartbeat_cb(std_msgs::msg::Empty::ConstSharedPtr msg) // TODO: This should be done in a separate thread void MAVManager::heartbeat() { + const float freq = 10; // Hz // Only need to do monitoring at the specified frequency rclcpp::Time t = this->now(); + + if (!last_heartbeat_t_initialized_) { + last_heartbeat_t_ = t; + return; + } + float dt = (t - last_heartbeat_t_).seconds(); if(dt < 1 / freq) return; @@ -963,20 +968,6 @@ bool MAVManager::transition(const std::string &tracker_str) if(!construction_done) { auto future = srv_transition_->async_send_request(transition_cmd); - //auto future = std::async(std::launch::async, &MAVManager::send_transition_request, this, transition_cmd); - //future.wait_for(std::chrono::seconds(1)); - //RCLCPP_INFO(this->get_logger(), "Not construction done."); - //RCLCPP_INFO(this->get_logger(), "Sent Tracker Transition request."); - //// future.wait_for(std::chrono::seconds(1)); - //if(future.get()) - //{ - // active_tracker_ = tracker_str; - // RCLCPP_INFO(this->get_logger(), "Current tracker: %s", tracker_str.c_str()); - // RCLCPP_INFO(this->get_logger(), "Returning from Transition"); - // return true; - //} - - rclcpp::FutureReturnCode frc = rclcpp::spin_until_future_complete(this->get_node_base_interface(), future); RCLCPP_INFO(this->get_logger(), "Done spinning until future complete"); if(frc == rclcpp::FutureReturnCode::SUCCESS) diff --git a/log3880012195.csv b/log3880012195.csv new file mode 100644 index 00000000..997decf7 --- /dev/null +++ b/log3880012195.csv @@ -0,0 +1,526 @@ +id,type,group,name +0,1,activeMarker,btSns +1,1,activeMarker,i2cOk +2,1,motion,motion +3,5,motion,deltaX +4,5,motion,deltaY +5,2,motion,shutter +6,1,motion,maxRaw +7,1,motion,minRaw +8,1,motion,Rawsum +9,1,motion,outlierCount +10,1,motion,squal +11,7,motion,std +12,7,ring,fadeTime +13,1,loco,mode +14,7,loco,spiWr +15,7,loco,spiRe +16,2,ranging,state +17,7,ranging,distance0 +18,7,ranging,distance1 +19,7,ranging,distance2 +20,7,ranging,distance3 +21,7,ranging,distance4 +22,7,ranging,distance5 +23,7,ranging,distance6 +24,7,ranging,distance7 +25,7,ranging,pressure0 +26,7,ranging,pressure1 +27,7,ranging,pressure2 +28,7,ranging,pressure3 +29,7,ranging,pressure4 +30,7,ranging,pressure5 +31,7,ranging,pressure6 +32,7,ranging,pressure7 +33,7,tdoa2,d7-0 +34,7,tdoa2,d0-1 +35,7,tdoa2,d1-2 +36,7,tdoa2,d2-3 +37,7,tdoa2,d3-4 +38,7,tdoa2,d4-5 +39,7,tdoa2,d5-6 +40,7,tdoa2,d6-7 +41,7,tdoa2,cc0 +42,7,tdoa2,cc1 +43,7,tdoa2,cc2 +44,7,tdoa2,cc3 +45,7,tdoa2,cc4 +46,7,tdoa2,cc5 +47,7,tdoa2,cc6 +48,7,tdoa2,cc7 +49,2,tdoa2,dist7-0 +50,2,tdoa2,dist0-1 +51,2,tdoa2,dist1-2 +52,2,tdoa2,dist2-3 +53,2,tdoa2,dist3-4 +54,2,tdoa2,dist4-5 +55,2,tdoa2,dist5-6 +56,2,tdoa2,dist6-7 +57,1,twr,rangingSuccessRate0 +58,1,twr,rangingPerSec0 +59,1,twr,rangingSuccessRate1 +60,1,twr,rangingPerSec1 +61,1,twr,rangingSuccessRate2 +62,1,twr,rangingPerSec2 +63,1,twr,rangingSuccessRate3 +64,1,twr,rangingPerSec3 +65,1,twr,rangingSuccessRate4 +66,1,twr,rangingPerSec4 +67,1,twr,rangingSuccessRate5 +68,1,twr,rangingPerSec5 +69,2,oa,front +70,2,oa,back +71,2,oa,up +72,2,oa,left +73,2,oa,right +74,7,usd,spiWrBps +75,7,usd,spiReBps +76,7,usd,fatWrBps +77,2,rpm,m1 +78,2,rpm,m2 +79,2,rpm,m3 +80,2,rpm,m4 +81,2,motor,m1 +82,2,motor,m2 +83,2,motor,m3 +84,2,motor,m4 +85,6,motor,m1req +86,6,motor,m2req +87,6,motor,m3req +88,6,motor,m4req +89,7,pm,vbat +90,2,pm,vbatMV +91,7,pm,extVbat +92,2,pm,extVbatMV +93,7,pm,extCurr +94,7,pm,chargeCurrent +95,4,pm,state +96,1,pm,batteryLevel +97,1,radio,rssi +98,1,radio,isConnected +99,2,radio,numRxBc +100,2,radio,numRxUc +101,5,gyro,xRaw +102,5,gyro,yRaw +103,5,gyro,zRaw +104,7,gyro,xVariance +105,7,gyro,yVariance +106,7,gyro,zVariance +107,7,gyro,x +108,7,gyro,y +109,7,gyro,z +110,3,colAv,latency +111,7,ext_pos,X +112,7,ext_pos,Y +113,7,ext_pos,Z +114,7,locSrv,x +115,7,locSrv,y +116,7,locSrv,z +117,7,locSrv,qx +118,7,locSrv,qy +119,7,locSrv,qz +120,7,locSrv,qw +121,2,locSrvZ,tick +122,2,crtp,rxRate +123,2,crtp,txRate +124,7,extrx,thrust +125,7,extrx,roll +126,7,extrx,pitch +127,7,extrx,rollRate +128,7,extrx,pitchRate +129,7,extrx,yawRate +130,7,extrx,zVel +131,1,extrx,AltHold +132,1,extrx,Arm +133,2,extrx_raw,ch0 +134,2,extrx_raw,ch1 +135,2,extrx_raw,ch2 +136,2,extrx_raw,ch3 +137,2,extrx_raw,ch4 +138,2,extrx_raw,ch5 +139,2,extrx_raw,ch6 +140,2,extrx_raw,ch7 +141,7,health,motorVarXM1 +142,7,health,motorVarYM1 +143,7,health,motorVarXM2 +144,7,health,motorVarYM2 +145,7,health,motorVarXM3 +146,7,health,motorVarYM3 +147,7,health,motorVarXM4 +148,7,health,motorVarYM4 +149,1,health,motorPass +150,7,health,batterySag +151,1,health,batteryPass +152,2,health,motorTestCount +153,3,memTst,errCntW +154,2,range,front +155,2,range,back +156,2,range,up +157,2,range,left +158,2,range,right +159,2,range,zrange +160,7,sensfusion6,qw +161,7,sensfusion6,qx +162,7,sensfusion6,qy +163,7,sensfusion6,qz +164,7,sensfusion6,gravityX +165,7,sensfusion6,gravityY +166,7,sensfusion6,gravityZ +167,7,sensfusion6,accZbase +168,1,sensfusion6,isInit +169,1,sensfusion6,isCalibrated +170,7,acc,x +171,7,acc,y +172,7,acc,z +173,7,baro,asl +174,7,baro,temp +175,7,baro,pressure +176,5,controller,ctr_yaw +177,7,controller,cmd_thrust +178,7,controller,cmd_roll +179,7,controller,cmd_pitch +180,7,controller,cmd_yaw +181,7,controller,r_roll +182,7,controller,r_pitch +183,7,controller,r_yaw +184,7,controller,accelz +185,7,controller,actuatorThrust +186,7,controller,roll +187,7,controller,pitch +188,7,controller,yaw +189,7,controller,rollRate +190,7,controller,pitchRate +191,7,controller,yawRate +192,7,ctrltarget,x +193,7,ctrltarget,y +194,7,ctrltarget,z +195,7,ctrltarget,vx +196,7,ctrltarget,vy +197,7,ctrltarget,vz +198,7,ctrltarget,ax +199,7,ctrltarget,ay +200,7,ctrltarget,az +201,7,ctrltarget,roll +202,7,ctrltarget,pitch +203,7,ctrltarget,yaw +204,5,ctrltargetZ,x +205,5,ctrltargetZ,y +206,5,ctrltargetZ,z +207,5,ctrltargetZ,vx +208,5,ctrltargetZ,vy +209,5,ctrltargetZ,vz +210,5,ctrltargetZ,ax +211,5,ctrltargetZ,ay +212,5,ctrltargetZ,az +213,7,mag,x +214,7,mag,y +215,7,mag,z +216,7,stabilizer,roll +217,7,stabilizer,pitch +218,7,stabilizer,yaw +219,7,stabilizer,thrust +220,7,stabilizer,rtStab +221,3,stabilizer,intToOut +222,7,stateEstimate,x +223,7,stateEstimate,y +224,7,stateEstimate,z +225,7,stateEstimate,vx +226,7,stateEstimate,vy +227,7,stateEstimate,vz +228,7,stateEstimate,ax +229,7,stateEstimate,ay +230,7,stateEstimate,az +231,7,stateEstimate,roll +232,7,stateEstimate,pitch +233,7,stateEstimate,yaw +234,7,stateEstimate,qx +235,7,stateEstimate,qy +236,7,stateEstimate,qz +237,7,stateEstimate,qw +238,5,stateEstimateZ,x +239,5,stateEstimateZ,y +240,5,stateEstimateZ,z +241,5,stateEstimateZ,vx +242,5,stateEstimateZ,vy +243,5,stateEstimateZ,vz +244,5,stateEstimateZ,ax +245,5,stateEstimateZ,ay +246,5,stateEstimateZ,az +247,3,stateEstimateZ,quat +248,5,stateEstimateZ,rateRoll +249,5,stateEstimateZ,ratePitch +250,5,stateEstimateZ,rateYaw +251,2,supervisor,info +252,1,sys,canfly +253,1,sys,isFlying +254,1,sys,isTumbled +255,4,sys,testLogParam +256,7,tdoaEngine,stRx +257,7,tdoaEngine,stEst +258,7,tdoaEngine,stTime +259,7,tdoaEngine,stFound +260,7,tdoaEngine,stCc +261,7,tdoaEngine,stHit +262,7,tdoaEngine,stMiss +263,7,tdoaEngine,cc +264,2,tdoaEngine,tof +265,7,tdoaEngine,tdoa +266,7,kalman_pred,predNX +267,7,kalman_pred,predNY +268,7,kalman_pred,measNX +269,7,kalman_pred,measNY +270,1,lighthouse,validAngles +271,7,lighthouse,rawAngle0x +272,7,lighthouse,rawAngle0y +273,7,lighthouse,rawAngle1x +274,7,lighthouse,rawAngle1y +275,7,lighthouse,angle0x +276,7,lighthouse,angle0y +277,7,lighthouse,angle1x +278,7,lighthouse,angle1y +279,7,lighthouse,angle0x_1 +280,7,lighthouse,angle0y_1 +281,7,lighthouse,angle1x_1 +282,7,lighthouse,angle1y_1 +283,7,lighthouse,angle0x_2 +284,7,lighthouse,angle0y_2 +285,7,lighthouse,angle1x_2 +286,7,lighthouse,angle1y_2 +287,7,lighthouse,angle0x_3 +288,7,lighthouse,angle0y_3 +289,7,lighthouse,angle1x_3 +290,7,lighthouse,angle1y_3 +291,7,lighthouse,rawAngle0xlh2 +292,7,lighthouse,rawAngle0ylh2 +293,7,lighthouse,rawAngle1xlh2 +294,7,lighthouse,rawAngle1ylh2 +295,7,lighthouse,angle0x_0lh2 +296,7,lighthouse,angle0y_0lh2 +297,7,lighthouse,angle1x_0lh2 +298,7,lighthouse,angle1y_0lh2 +299,2,lighthouse,width0 +300,2,lighthouse,width1 +301,2,lighthouse,width2 +302,2,lighthouse,width3 +303,1,lighthouse,comSync +304,2,lighthouse,bsAvailable +305,2,lighthouse,bsReceive +306,2,lighthouse,bsActive +307,2,lighthouse,bsCalUd +308,2,lighthouse,bsCalCon +309,1,lighthouse,status +310,7,lighthouse,posRt +311,7,lighthouse,estBs0Rt +312,7,lighthouse,estBs1Rt +313,7,lighthouse,x +314,7,lighthouse,y +315,7,lighthouse,z +316,7,lighthouse,delta +317,2,lighthouse,bsGeoVal +318,2,lighthouse,bsCalVal +319,7,lighthouse,disProb +320,7,pid_attitude,roll_outP +321,7,pid_attitude,roll_outI +322,7,pid_attitude,roll_outD +323,7,pid_attitude,roll_outFF +324,7,pid_attitude,pitch_outP +325,7,pid_attitude,pitch_outI +326,7,pid_attitude,pitch_outD +327,7,pid_attitude,pitch_outFF +328,7,pid_attitude,yaw_outP +329,7,pid_attitude,yaw_outI +330,7,pid_attitude,yaw_outD +331,7,pid_attitude,yaw_outFF +332,7,pid_rate,roll_outP +333,7,pid_rate,roll_outI +334,7,pid_rate,roll_outD +335,7,pid_rate,roll_outFF +336,7,pid_rate,pitch_outP +337,7,pid_rate,pitch_outI +338,7,pid_rate,pitch_outD +339,7,pid_rate,pitch_outFF +340,7,pid_rate,yaw_outP +341,7,pid_rate,yaw_outI +342,7,pid_rate,yaw_outD +343,7,pid_rate,yaw_outFF +344,7,ctrlINDI,cmd_thrust +345,7,ctrlINDI,cmd_roll +346,7,ctrlINDI,cmd_pitch +347,7,ctrlINDI,cmd_yaw +348,7,ctrlINDI,r_roll +349,7,ctrlINDI,r_pitch +350,7,ctrlINDI,r_yaw +351,7,ctrlINDI,u_act_dyn_p +352,7,ctrlINDI,u_act_dyn_q +353,7,ctrlINDI,u_act_dyn_r +354,7,ctrlINDI,du_p +355,7,ctrlINDI,du_q +356,7,ctrlINDI,du_r +357,7,ctrlINDI,ang_accel_ref_p +358,7,ctrlINDI,ang_accel_ref_q +359,7,ctrlINDI,ang_accel_ref_r +360,7,ctrlINDI,rate_d[0] +361,7,ctrlINDI,rate_d[1] +362,7,ctrlINDI,rate_d[2] +363,7,ctrlINDI,uf_p +364,7,ctrlINDI,uf_q +365,7,ctrlINDI,uf_r +366,7,ctrlINDI,Omega_f_p +367,7,ctrlINDI,Omega_f_q +368,7,ctrlINDI,Omega_f_r +369,7,ctrlINDI,n_p +370,7,ctrlINDI,n_q +371,7,ctrlINDI,n_r +372,7,ctrlMel,cmd_thrust +373,7,ctrlMel,cmd_roll +374,7,ctrlMel,cmd_pitch +375,7,ctrlMel,cmd_yaw +376,7,ctrlMel,r_roll +377,7,ctrlMel,r_pitch +378,7,ctrlMel,r_yaw +379,7,ctrlMel,accelz +380,7,ctrlMel,zdx +381,7,ctrlMel,zdy +382,7,ctrlMel,zdz +383,7,ctrlMel,i_err_x +384,7,ctrlMel,i_err_y +385,7,ctrlMel,i_err_z +386,7,posCtrlIndi,posRef_x +387,7,posCtrlIndi,posRef_y +388,7,posCtrlIndi,posRef_z +389,7,posCtrlIndi,velS_x +390,7,posCtrlIndi,velS_y +391,7,posCtrlIndi,velS_z +392,7,posCtrlIndi,velRef_x +393,7,posCtrlIndi,velRef_y +394,7,posCtrlIndi,velRef_z +395,7,posCtrlIndi,angS_roll +396,7,posCtrlIndi,angS_pitch +397,7,posCtrlIndi,angS_yaw +398,7,posCtrlIndi,angF_roll +399,7,posCtrlIndi,angF_pitch +400,7,posCtrlIndi,angF_yaw +401,7,posCtrlIndi,accRef_x +402,7,posCtrlIndi,accRef_y +403,7,posCtrlIndi,accRef_z +404,7,posCtrlIndi,accS_x +405,7,posCtrlIndi,accS_y +406,7,posCtrlIndi,accS_z +407,7,posCtrlIndi,accF_x +408,7,posCtrlIndi,accF_y +409,7,posCtrlIndi,accF_z +410,7,posCtrlIndi,accFT_x +411,7,posCtrlIndi,accFT_y +412,7,posCtrlIndi,accFT_z +413,7,posCtrlIndi,accErr_x +414,7,posCtrlIndi,accErr_y +415,7,posCtrlIndi,accErr_z +416,7,posCtrlIndi,phi_tilde +417,7,posCtrlIndi,theta_tilde +418,7,posCtrlIndi,T_tilde +419,7,posCtrlIndi,T_inner +420,7,posCtrlIndi,T_inner_f +421,7,posCtrlIndi,T_incremented +422,7,posCtrlIndi,cmd_phi +423,7,posCtrlIndi,cmd_theta +424,7,posCtl,targetVX +425,7,posCtl,targetVY +426,7,posCtl,targetVZ +427,7,posCtl,targetX +428,7,posCtl,targetY +429,7,posCtl,targetZ +430,7,posCtl,bodyVX +431,7,posCtl,bodyVY +432,7,posCtl,bodyX +433,7,posCtl,bodyY +434,7,posCtl,Xp +435,7,posCtl,Xi +436,7,posCtl,Xd +437,7,posCtl,Xff +438,7,posCtl,Yp +439,7,posCtl,Yi +440,7,posCtl,Yd +441,7,posCtl,Yff +442,7,posCtl,Zp +443,7,posCtl,Zi +444,7,posCtl,Zd +445,7,posCtl,Zff +446,7,posCtl,VXp +447,7,posCtl,VXi +448,7,posCtl,VXd +449,7,posCtl,VXff +450,7,posCtl,VYp +451,7,posCtl,VYi +452,7,posCtl,VYd +453,7,posCtl,VYff +454,7,posCtl,VZp +455,7,posCtl,VZi +456,7,posCtl,VZd +457,7,posCtl,VZff +458,7,ctrlLee,KR_x +459,7,ctrlLee,KR_y +460,7,ctrlLee,KR_z +461,7,ctrlLee,Kw_x +462,7,ctrlLee,Kw_y +463,7,ctrlLee,Kw_z +464,7,ctrlLee,Kpos_Px +465,7,ctrlLee,Kpos_Py +466,7,ctrlLee,Kpos_Pz +467,7,ctrlLee,Kpos_Dx +468,7,ctrlLee,Kpos_Dy +469,7,ctrlLee,Kpos_Dz +470,7,ctrlLee,thrustSi +471,7,ctrlLee,torquex +472,7,ctrlLee,torquey +473,7,ctrlLee,torquez +474,7,ctrlLee,rpyx +475,7,ctrlLee,rpyy +476,7,ctrlLee,rpyz +477,7,ctrlLee,rpydx +478,7,ctrlLee,rpydy +479,7,ctrlLee,rpydz +480,7,ctrlLee,error_posx +481,7,ctrlLee,error_posy +482,7,ctrlLee,error_posz +483,7,ctrlLee,error_velx +484,7,ctrlLee,error_vely +485,7,ctrlLee,error_velz +486,7,ctrlLee,omegax +487,7,ctrlLee,omegay +488,7,ctrlLee,omegaz +489,7,ctrlLee,omegarx +490,7,ctrlLee,omegary +491,7,ctrlLee,omegarz +492,7,kalman,stateX +493,7,kalman,stateY +494,7,kalman,stateZ +495,7,kalman,statePX +496,7,kalman,statePY +497,7,kalman,statePZ +498,7,kalman,stateD0 +499,7,kalman,stateD1 +500,7,kalman,stateD2 +501,7,kalman,varX +502,7,kalman,varY +503,7,kalman,varZ +504,7,kalman,varPX +505,7,kalman,varPY +506,7,kalman,varPZ +507,7,kalman,varD0 +508,7,kalman,varD1 +509,7,kalman,varD2 +510,7,kalman,q0 +511,7,kalman,q1 +512,7,kalman,q2 +513,7,kalman,q3 +514,7,kalman,rtUpdate +515,7,kalman,rtPred +516,7,kalman,rtFinal +517,6,outlierf,lhWin +518,7,estimator,rtApnd +519,7,estimator,rtRej +520,7,posEstAlt,estimatedZ +521,7,posEstAlt,estVZ +522,7,posEstAlt,velocityZ +523,1,DTR_P2P,rx_state +524,1,DTR_P2P,tx_state diff --git a/params3679191350.csv b/params3679191350.csv new file mode 100644 index 00000000..9e807627 --- /dev/null +++ b/params3679191350.csv @@ -0,0 +1,311 @@ +id,type,readonly,group,name +0,8,0,radiotest,channel +1,0,0,radiotest,power +2,8,0,radiotest,contwave +3,8,0,activeMarker,front +4,8,0,activeMarker,back +5,8,0,activeMarker,left +6,8,0,activeMarker,right +7,8,0,activeMarker,mode +8,8,0,activeMarker,poll +9,8,1,deck,bcActiveMarker +10,8,1,deck,bcAI +11,8,1,deck,bcBuzzer +12,8,1,deck,bcFlow +13,8,1,deck,bcFlow2 +14,8,1,deck,bcLedRing +15,8,1,deck,bcLighthouse4 +16,8,1,deck,bcDWM1000 +17,8,1,deck,bcLoco +18,8,1,deck,bcMultiranger +19,8,1,deck,bcOA +20,8,1,deck,bcUSD +21,8,1,deck,bcZRanger +22,8,1,deck,bcZRanger2 +23,8,0,motion,disable +24,8,0,motion,adaptive +25,6,0,motion,flowStdFixed +26,8,0,ring,effect +27,10,1,ring,neffect +28,8,0,ring,solidRed +29,8,0,ring,solidGreen +30,8,0,ring,solidBlue +31,8,0,ring,headlightEnable +32,6,0,ring,emptyCharge +33,6,0,ring,fullCharge +34,10,0,ring,fadeColor +35,6,0,ring,fadeTime +36,8,0,system,highlight +37,8,0,system,storageStats +38,8,0,system,storageReformat +39,8,0,system,taskDump +40,0,1,system,selftestPassed +41,8,0,system,assertInfo +42,8,0,system,testLogParam +43,8,0,system,doAssert +44,8,0,loco,mode +45,6,0,tdoa2,stddev +46,6,0,tdoa3,stddev +47,9,0,multiranger,filterMask +48,8,1,usd,canLog +49,8,0,usd,logging +50,8,0,led,bitmask +51,8,0,motorPowerSet,enable +52,9,0,motorPowerSet,m1 +53,9,0,motorPowerSet,m2 +54,9,0,motorPowerSet,m3 +55,9,0,motorPowerSet,m4 +56,6,0,pm,lowVoltage +57,6,0,pm,criticalLowVoltage +58,8,1,imu_sensors,BMP3XX +59,6,0,imu_sensors,imuPhi +60,6,0,imu_sensors,imuTheta +61,6,0,imu_sensors,imuPsi +62,8,1,imu_sensors,AK8963 +63,8,1,imu_sensors,LPS25H +64,8,1,imu_tests,MPU6500 +65,8,1,imu_tests,AK8963 +66,8,1,imu_tests,LPS25H +67,6,0,imu_tests,imuPhi +68,6,0,imu_tests,imuTheta +69,6,0,imu_tests,imuPsi +70,8,0,syslink,probe +71,8,0,usec,reset +72,8,0,colAv,enable +73,6,0,colAv,ellipsoidX +74,6,0,colAv,ellipsoidY +75,6,0,colAv,ellipsoidZ +76,6,0,colAv,bboxMinX +77,6,0,colAv,bboxMinY +78,6,0,colAv,bboxMinZ +79,6,0,colAv,bboxMaxX +80,6,0,colAv,bboxMaxY +81,6,0,colAv,bboxMaxZ +82,6,0,colAv,horizon +83,6,0,colAv,maxSpeed +84,6,0,colAv,sidestepThrsh +85,2,0,colAv,maxPeerLocAge +86,6,0,colAv,vorTol +87,2,0,colAv,vorIters +88,8,0,commander,enHighLevel +89,6,0,cppm,rateRoll +90,6,0,cppm,ratePitch +91,6,0,cppm,angPitch +92,6,0,cppm,angRoll +93,6,0,cppm,rateYaw +94,6,0,hlCommander,vtoff +95,6,0,hlCommander,vland +96,8,0,hlCommander,groupmask +97,8,0,flightmode,althold +98,8,0,flightmode,poshold +99,8,0,flightmode,posSet +100,8,0,flightmode,yawMode +101,8,0,flightmode,stabModeRoll +102,8,0,flightmode,stabModePitch +103,8,0,flightmode,stabModeYaw +104,8,0,locSrv,enRangeStreamFP32 +105,8,0,locSrv,enLhAngleStream +106,6,0,locSrv,extPosStdDev +107,6,0,locSrv,extQuatStdDev +108,9,0,crtpsrv,echoDelay +109,8,0,health,startPropTest +110,8,0,health,startBatTest +111,9,0,health,propTestPWMRatio +112,9,0,health,batTestPWMRatio +113,6,0,kalman,maxPos +114,6,0,kalman,maxVel +115,8,0,kalman,resetEstimation +116,8,0,kalman,robustTdoa +117,8,0,kalman,robustTwr +118,6,0,kalman,pNAcc_xy +119,6,0,kalman,pNAcc_z +120,6,0,kalman,pNVel +121,6,0,kalman,pNPos +122,6,0,kalman,pNAtt +123,6,0,kalman,mNBaro +124,6,0,kalman,mNGyro_rollpitch +125,6,0,kalman,mNGyro_yaw +126,6,0,kalman,initialX +127,6,0,kalman,initialY +128,6,0,kalman,initialZ +129,6,0,kalman,initialYaw +130,8,0,memTst,resetW +131,10,0,powerDist,idleThrust +132,6,0,quadSysId,thrustToTorque +133,6,0,quadSysId,pwmToThrustA +134,6,0,quadSysId,pwmToThrustB +135,6,0,quadSysId,armLength +136,6,0,sensfusion6,kp +137,6,0,sensfusion6,ki +138,6,0,sensfusion6,baseZacc +139,8,0,sound,effect +140,10,1,sound,neffect +141,9,0,sound,freq +142,8,0,stabilizer,estimator +143,8,0,stabilizer,controller +144,8,0,stabilizer,stop +145,8,0,supervisor,infdmp +146,9,0,supervisor,prefltTimeout +147,9,0,supervisor,landedTimeout +148,8,0,supervisor,tmblChckEn +149,9,1,cpu,flash +150,10,1,cpu,id0 +151,10,1,cpu,id1 +152,10,1,cpu,id2 +153,8,0,tdoaEngine,logId +154,8,0,tdoaEngine,logOthrId +155,8,0,tdoaEngine,matchAlgo +156,8,0,lighthouse,method +157,8,0,lighthouse,bsCalibReset +158,8,0,lighthouse,systemType +159,9,1,lighthouse,bsAvailable +160,6,0,lighthouse,sweepStd +161,6,0,lighthouse,sweepStd2 +162,8,0,lighthouse,enLhRawStream +163,9,0,lighthouse,lh2maxRate +164,6,0,pid_attitude,roll_kp +165,6,0,pid_attitude,roll_ki +166,6,0,pid_attitude,roll_kd +167,6,0,pid_attitude,roll_kff +168,6,0,pid_attitude,pitch_kp +169,6,0,pid_attitude,pitch_ki +170,6,0,pid_attitude,pitch_kd +171,6,0,pid_attitude,pitch_kff +172,6,0,pid_attitude,yaw_kp +173,6,0,pid_attitude,yaw_ki +174,6,0,pid_attitude,yaw_kd +175,6,0,pid_attitude,yaw_kff +176,6,0,pid_attitude,yawMaxDelta +177,0,0,pid_attitude,attFiltEn +178,6,0,pid_attitude,attFiltCut +179,6,0,pid_rate,roll_kp +180,6,0,pid_rate,roll_ki +181,6,0,pid_rate,roll_kd +182,6,0,pid_rate,roll_kff +183,6,0,pid_rate,pitch_kp +184,6,0,pid_rate,pitch_ki +185,6,0,pid_rate,pitch_kd +186,6,0,pid_rate,pitch_kff +187,6,0,pid_rate,yaw_kp +188,6,0,pid_rate,yaw_ki +189,6,0,pid_rate,yaw_kd +190,6,0,pid_rate,yaw_kff +191,0,0,pid_rate,rateFiltEn +192,6,0,pid_rate,omxFiltCut +193,6,0,pid_rate,omyFiltCut +194,6,0,pid_rate,omzFiltCut +195,6,0,ctrlINDI,thrust_threshold +196,6,0,ctrlINDI,bound_ctrl_input +197,6,0,ctrlINDI,g1_p +198,6,0,ctrlINDI,g1_q +199,6,0,ctrlINDI,g1_r +200,6,0,ctrlINDI,g2 +201,6,0,ctrlINDI,ref_err_p +202,6,0,ctrlINDI,ref_err_q +203,6,0,ctrlINDI,ref_err_r +204,6,0,ctrlINDI,ref_rate_p +205,6,0,ctrlINDI,ref_rate_q +206,6,0,ctrlINDI,ref_rate_r +207,6,0,ctrlINDI,act_dyn_p +208,6,0,ctrlINDI,act_dyn_q +209,6,0,ctrlINDI,act_dyn_r +210,6,0,ctrlINDI,filt_cutoff +211,6,0,ctrlINDI,filt_cutoff_r +212,8,0,ctrlINDI,outerLoopActive +213,6,0,ctrlMel,kp_xy +214,6,0,ctrlMel,kd_xy +215,6,0,ctrlMel,ki_xy +216,6,0,ctrlMel,i_range_xy +217,6,0,ctrlMel,kp_z +218,6,0,ctrlMel,kd_z +219,6,0,ctrlMel,ki_z +220,6,0,ctrlMel,i_range_z +221,6,0,ctrlMel,mass +222,6,0,ctrlMel,massThrust +223,6,0,ctrlMel,kR_xy +224,6,0,ctrlMel,kR_z +225,6,0,ctrlMel,kw_xy +226,6,0,ctrlMel,kw_z +227,6,0,ctrlMel,ki_m_xy +228,6,0,ctrlMel,ki_m_z +229,6,0,ctrlMel,kd_omega_rp +230,6,0,ctrlMel,i_range_m_xy +231,6,0,ctrlMel,i_range_m_z +232,6,0,ctrlAtt,tau_xy +233,6,0,ctrlAtt,zeta_xy +234,6,0,ctrlAtt,tau_z +235,6,0,ctrlAtt,zeta_z +236,6,0,ctrlAtt,tau_rp +237,6,0,ctrlAtt,mixing_factor +238,6,0,ctrlAtt,coll_fairness +239,6,0,posCtrlIndi,K_xi_x +240,6,0,posCtrlIndi,K_xi_y +241,6,0,posCtrlIndi,K_xi_z +242,6,0,posCtrlIndi,K_dxi_x +243,6,0,posCtrlIndi,K_dxi_y +244,6,0,posCtrlIndi,K_dxi_z +245,6,0,posCtrlIndi,pq_clamping +246,6,0,posCtlPid,xKp +247,6,0,posCtlPid,xKi +248,6,0,posCtlPid,xKd +249,6,0,posCtlPid,xKff +250,6,0,posCtlPid,yKp +251,6,0,posCtlPid,yKi +252,6,0,posCtlPid,yKd +253,6,0,posCtlPid,yKff +254,6,0,posCtlPid,zKp +255,6,0,posCtlPid,zKi +256,6,0,posCtlPid,zKd +257,6,0,posCtlPid,zKff +258,9,0,posCtlPid,thrustBase +259,9,0,posCtlPid,thrustMin +260,6,0,posCtlPid,rLimit +261,6,0,posCtlPid,pLimit +262,6,0,posCtlPid,xVelMax +263,6,0,posCtlPid,yVelMax +264,6,0,posCtlPid,zVelMax +265,6,0,velCtlPid,vxKp +266,6,0,velCtlPid,vxKi +267,6,0,velCtlPid,vxKd +268,6,0,velCtlPid,vxKFF +269,6,0,velCtlPid,vyKp +270,6,0,velCtlPid,vyKi +271,6,0,velCtlPid,vyKd +272,6,0,velCtlPid,vyKFF +273,6,0,velCtlPid,vzKp +274,6,0,velCtlPid,vzKi +275,6,0,velCtlPid,vzKd +276,6,0,velCtlPid,vzKFF +277,6,0,ctrlLee,KR_x +278,6,0,ctrlLee,KR_y +279,6,0,ctrlLee,KR_z +280,6,0,ctrlLee,KI_x +281,6,0,ctrlLee,KI_y +282,6,0,ctrlLee,KI_z +283,6,0,ctrlLee,Kw_x +284,6,0,ctrlLee,Kw_y +285,6,0,ctrlLee,Kw_z +286,6,0,ctrlLee,J_x +287,6,0,ctrlLee,J_y +288,6,0,ctrlLee,J_z +289,6,0,ctrlLee,Kpos_Px +290,6,0,ctrlLee,Kpos_Py +291,6,0,ctrlLee,Kpos_Pz +292,6,0,ctrlLee,Kpos_P_limit +293,6,0,ctrlLee,Kpos_Dx +294,6,0,ctrlLee,Kpos_Dy +295,6,0,ctrlLee,Kpos_Dz +296,6,0,ctrlLee,Kpos_D_limit +297,6,0,ctrlLee,Kpos_Ix +298,6,0,ctrlLee,Kpos_Iy +299,6,0,ctrlLee,Kpos_Iz +300,6,0,ctrlLee,Kpos_I_limit +301,6,0,ctrlLee,mass +302,6,0,posEstAlt,estAlphaAsl +303,6,0,posEstAlt,estAlphaZr +304,6,0,posEstAlt,velFactor +305,6,0,posEstAlt,velZAlpha +306,6,0,posEstAlt,vAccDeadband +307,10,1,firmware,revision0 +308,9,1,firmware,revision1 +309,8,1,firmware,modified From 8e97a7eb6ba90accc02eff864a645ef2abc3f8d1 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Wed, 19 Mar 2025 16:58:15 -0400 Subject: [PATCH 31/64] various issues fixed, still not taking off --- .../launch/cf_test.launch.py | 4 ++++ kr_mav_manager/src/manager.cpp | 4 ++-- .../src/line_tracker_distance_server.cpp | 14 ++++++++++++++ .../kr_trackers_manager/launch/example.launch.py | 1 + .../kr_trackers_manager/src/trackers_manager.cpp | 2 +- 5 files changed, 22 insertions(+), 3 deletions(-) diff --git a/interfaces/kr_crazyflie_interface/launch/cf_test.launch.py b/interfaces/kr_crazyflie_interface/launch/cf_test.launch.py index ec3949e7..32431884 100644 --- a/interfaces/kr_crazyflie_interface/launch/cf_test.launch.py +++ b/interfaces/kr_crazyflie_interface/launch/cf_test.launch.py @@ -119,6 +119,7 @@ def generate_launch_description(): DeclareLaunchArgument('robot', default_value='cf3'), DeclareLaunchArgument('odom', default_value='odom'), DeclareLaunchArgument('so3_cmd', default_value='so3_cmd'), + DeclareLaunchArgument('mass', default_value='.030'), ] # Initialize launch description with all arguments @@ -187,6 +188,9 @@ def generate_launch_description(): namespace=LaunchConfiguration('robot'), name="mav_services", output='screen', + parameters = [ + {'mass': LaunchConfiguration('mass')}, + ] ) ) diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 5cee067e..550432c7 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -283,7 +283,7 @@ bool MAVManager::takeoff() goal.relative = true; 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); + auto future = line_tracker_distance_client_->async_send_goal(goal, options); if(this->transition(line_tracker_distance)) { @@ -989,7 +989,7 @@ bool MAVManager::transition(const std::string &tracker_str) RCLCPP_INFO(this->get_logger(), "Construction done."); RCLCPP_INFO(this->get_logger(), "Sent Tracker Transition request"); auto future = std::async(std::launch::async, &MAVManager::send_transition_request, this, transition_cmd); - future.wait_for(std::chrono::seconds(1)); + // future.wait_for(std::chrono::seconds(1)); if(future.get()) { active_tracker_ = tracker_str; diff --git a/trackers/kr_trackers/src/line_tracker_distance_server.cpp b/trackers/kr_trackers/src/line_tracker_distance_server.cpp index ec0a6e30..bb6b6b7d 100644 --- a/trackers/kr_trackers/src/line_tracker_distance_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_distance_server.cpp @@ -103,9 +103,13 @@ void LineTrackerDistance::Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &p bool LineTrackerDistance::Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) { + RCLCPP_INFO(logger_, "In Activate 1"); (void)cmd; + RCLCPP_INFO_STREAM(logger_, "pos_set_: " << pos_set_ ); + RCLCPP_INFO_STREAM(logger_, "goal_set_: " << goal_set_ ); if(goal_set_ && pos_set_) { + RCLCPP_INFO(logger_, "In Activate 2"); { std::lock_guard lock(mutex_); if(!current_goal_handle_ || !current_goal_handle_->is_active()) @@ -304,6 +308,13 @@ rclcpp_action::GoalResponse LineTrackerDistance::goal_callback(const rclcpp_acti // If another goal is already active, cancel that goal // and track this one instead std::lock_guard lock(mutex_); + RCLCPP_INFO_STREAM(logger_, "current_goal_handle_: " << current_goal_handle_); + + if (current_goal_handle_ == 0) { + RCLCPP_INFO_STREAM(logger_, "Initialization of line tracker detected"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + if(current_goal_handle_ && current_goal_handle_->is_active()) { RCLCPP_INFO(logger_, "LineTrackerDistance goal (%2.2f, %2.2f, %2.2f) preempted.", goal_(0), goal_(1), goal_(2)); @@ -315,6 +326,7 @@ rclcpp_action::GoalResponse LineTrackerDistance::goal_callback(const rclcpp_acti goal_reached_ = false; } + RCLCPP_INFO(logger_, "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -345,6 +357,7 @@ void LineTrackerDistance::handle_accepted_callback(const std::shared_ptrget_goal(); @@ -373,6 +386,7 @@ void LineTrackerDistance::handle_accepted_callback(const std::shared_ptrcreate_subscription("~/odom", qos, std::bind(&TrackersManager::odom_callback, this, std::placeholders::_1)); + sub_odom_ = this->create_subscription("/cf3/odom", qos, std::bind(&TrackersManager::odom_callback, this, std::placeholders::_1)); srv_tracker_ = this->create_service("~/transition", std::bind(&TrackersManager::transition_callback, this, std::placeholders::_1, std::placeholders::_2)); From 8a52b17991a3304814173dd033c2d944cd7faddf Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Mon, 24 Mar 2025 18:41:53 -0400 Subject: [PATCH 32/64] fixed race condition, new issue for takeoff --- .../include/kr_mav_manager/manager.hpp | 3 +- kr_mav_manager/src/manager.cpp | 68 ++- .../src/line_tracker_distance_server.cpp | 8 +- .../kr_trackers_manager/log3880012195.csv | 526 ++++++++++++++++++ .../kr_trackers_manager/params3679191350.csv | 311 +++++++++++ 5 files changed, 903 insertions(+), 13 deletions(-) create mode 100644 trackers/kr_trackers_manager/log3880012195.csv create mode 100644 trackers/kr_trackers_manager/params3679191350.csv diff --git a/kr_mav_manager/include/kr_mav_manager/manager.hpp b/kr_mav_manager/include/kr_mav_manager/manager.hpp index 8ef66de0..6afd6b72 100644 --- a/kr_mav_manager/include/kr_mav_manager/manager.hpp +++ b/kr_mav_manager/include/kr_mav_manager/manager.hpp @@ -85,6 +85,7 @@ class MAVManager : public rclcpp::Node // Movement bool takeoff(); + bool send_takeoff_request(std::shared_ptr); bool goTo(float x, float y, float z, float yaw, float v_des = 0.0f, float a_des = 0.0f, bool relative = false); bool goTo(Vec4 xyz_yaw, Vec2 v_and_a_des = Vec2::Zero()); @@ -177,7 +178,7 @@ class MAVManager : public rclcpp::Node float home_yaw_; bool need_imu_, need_output_data_, need_odom_, use_attitude_safety_catch_, last_heartbeat_t_initialized_; - bool home_set_, motors_; + bool home_set_, motors_, goal_sent_; float voltage_, pressure_height_, pressure_dheight_; std::array magnetic_field_; std::array radio_; diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 550432c7..bbaa325d 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -281,19 +281,67 @@ bool MAVManager::takeoff() auto goal = LineTracker::Goal(); goal.z = takeoff_height_; goal.relative = true; - auto options = rclcpp_action::Client::SendGoalOptions(); - options.result_callback = std::bind(&MAVManager::tracker_done_callback, this, _1); - auto future = line_tracker_distance_client_->async_send_goal(goal, options); + // Create goal options with lambda callbacks + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // Goal response callback + send_goal_options.goal_response_callback = + [this](const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle) { + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } + }; + + // Result callback + send_goal_options.result_callback = + [this](const rclcpp_action::ClientGoalHandle::WrappedResult & result) { + RCLCPP_INFO(this->get_logger(), " IN LAMBDA"); + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + RCLCPP_INFO(this->get_logger(), "Takeoff successful"); + if(this->transition(line_tracker_distance)) + { + status_ = FLYING; + return true; + } + else + return false; + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Takeoff aborted"); + break; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Takeoff canceled"); + break; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + break; + } + }; + line_tracker_distance_client_->async_send_goal(goal, send_goal_options); + return true; - if(this->transition(line_tracker_distance)) - { - status_ = FLYING; - return true; - } - else - return false; + // auto future = std::async(std::launch::async, &MAVManager::send_takeoff_request, this, goal); + // if(future.get()) + // { + // RCLCPP_INFO(this->get_logger(), "got trueeeeeee"); + // } + // return false; } +// bool MAVManager::send_takeoff_request(std::shared_ptr goal) +// { +// RCLCPP_INFO(this->get_logger(), "IN SEND TAKEOFF REQ"); +// auto future = line_tracker_distance_client_->async_send_goal(*goal); +// auto status = future.wait_for(std::chrono::seconds(5)); +// if (status == std::future_status::timeout) { +// RCLCPP_ERROR(this->get_logger(), "Future timed out waiting for goal response"); +// return false; +// } +// } + bool MAVManager::set_mass(float m) { if(m > 0) diff --git a/trackers/kr_trackers/src/line_tracker_distance_server.cpp b/trackers/kr_trackers/src/line_tracker_distance_server.cpp index bb6b6b7d..8baedb76 100644 --- a/trackers/kr_trackers/src/line_tracker_distance_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_distance_server.cpp @@ -33,7 +33,7 @@ class LineTrackerDistance : public kr_trackers_manager::Tracker 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_action::ResultCode handle_accepted_callback(const std::shared_ptr goal_handle); rclcpp_action::Server::SharedPtr tracker_server_; rclcpp::CallbackGroup::SharedPtr cb_group_; @@ -312,6 +312,7 @@ rclcpp_action::GoalResponse LineTrackerDistance::goal_callback(const rclcpp_acti if (current_goal_handle_ == 0) { RCLCPP_INFO_STREAM(logger_, "Initialization of line tracker detected"); + goal_set_ = false; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -342,7 +343,7 @@ rclcpp_action::CancelResponse LineTrackerDistance::cancel_callback(const std::sh return rclcpp_action::CancelResponse::ACCEPT; } -void LineTrackerDistance::handle_accepted_callback(const std::shared_ptr goal_handle) +rclcpp_action::ResultCode LineTrackerDistance::handle_accepted_callback(const std::shared_ptr goal_handle) { std::lock_guard lock(mutex_); @@ -387,6 +388,9 @@ void LineTrackerDistance::handle_accepted_callback(const std::shared_ptr(); + goal_handle->succeed(result); + return rclcpp_action::ResultCode::SUCCEEDED; } uint8_t LineTrackerDistance::status() diff --git a/trackers/kr_trackers_manager/log3880012195.csv b/trackers/kr_trackers_manager/log3880012195.csv new file mode 100644 index 00000000..997decf7 --- /dev/null +++ b/trackers/kr_trackers_manager/log3880012195.csv @@ -0,0 +1,526 @@ +id,type,group,name +0,1,activeMarker,btSns +1,1,activeMarker,i2cOk +2,1,motion,motion +3,5,motion,deltaX +4,5,motion,deltaY +5,2,motion,shutter +6,1,motion,maxRaw +7,1,motion,minRaw +8,1,motion,Rawsum +9,1,motion,outlierCount +10,1,motion,squal +11,7,motion,std +12,7,ring,fadeTime +13,1,loco,mode +14,7,loco,spiWr +15,7,loco,spiRe +16,2,ranging,state +17,7,ranging,distance0 +18,7,ranging,distance1 +19,7,ranging,distance2 +20,7,ranging,distance3 +21,7,ranging,distance4 +22,7,ranging,distance5 +23,7,ranging,distance6 +24,7,ranging,distance7 +25,7,ranging,pressure0 +26,7,ranging,pressure1 +27,7,ranging,pressure2 +28,7,ranging,pressure3 +29,7,ranging,pressure4 +30,7,ranging,pressure5 +31,7,ranging,pressure6 +32,7,ranging,pressure7 +33,7,tdoa2,d7-0 +34,7,tdoa2,d0-1 +35,7,tdoa2,d1-2 +36,7,tdoa2,d2-3 +37,7,tdoa2,d3-4 +38,7,tdoa2,d4-5 +39,7,tdoa2,d5-6 +40,7,tdoa2,d6-7 +41,7,tdoa2,cc0 +42,7,tdoa2,cc1 +43,7,tdoa2,cc2 +44,7,tdoa2,cc3 +45,7,tdoa2,cc4 +46,7,tdoa2,cc5 +47,7,tdoa2,cc6 +48,7,tdoa2,cc7 +49,2,tdoa2,dist7-0 +50,2,tdoa2,dist0-1 +51,2,tdoa2,dist1-2 +52,2,tdoa2,dist2-3 +53,2,tdoa2,dist3-4 +54,2,tdoa2,dist4-5 +55,2,tdoa2,dist5-6 +56,2,tdoa2,dist6-7 +57,1,twr,rangingSuccessRate0 +58,1,twr,rangingPerSec0 +59,1,twr,rangingSuccessRate1 +60,1,twr,rangingPerSec1 +61,1,twr,rangingSuccessRate2 +62,1,twr,rangingPerSec2 +63,1,twr,rangingSuccessRate3 +64,1,twr,rangingPerSec3 +65,1,twr,rangingSuccessRate4 +66,1,twr,rangingPerSec4 +67,1,twr,rangingSuccessRate5 +68,1,twr,rangingPerSec5 +69,2,oa,front +70,2,oa,back +71,2,oa,up +72,2,oa,left +73,2,oa,right +74,7,usd,spiWrBps +75,7,usd,spiReBps +76,7,usd,fatWrBps +77,2,rpm,m1 +78,2,rpm,m2 +79,2,rpm,m3 +80,2,rpm,m4 +81,2,motor,m1 +82,2,motor,m2 +83,2,motor,m3 +84,2,motor,m4 +85,6,motor,m1req +86,6,motor,m2req +87,6,motor,m3req +88,6,motor,m4req +89,7,pm,vbat +90,2,pm,vbatMV +91,7,pm,extVbat +92,2,pm,extVbatMV +93,7,pm,extCurr +94,7,pm,chargeCurrent +95,4,pm,state +96,1,pm,batteryLevel +97,1,radio,rssi +98,1,radio,isConnected +99,2,radio,numRxBc +100,2,radio,numRxUc +101,5,gyro,xRaw +102,5,gyro,yRaw +103,5,gyro,zRaw +104,7,gyro,xVariance +105,7,gyro,yVariance +106,7,gyro,zVariance +107,7,gyro,x +108,7,gyro,y +109,7,gyro,z +110,3,colAv,latency +111,7,ext_pos,X +112,7,ext_pos,Y +113,7,ext_pos,Z +114,7,locSrv,x +115,7,locSrv,y +116,7,locSrv,z +117,7,locSrv,qx +118,7,locSrv,qy +119,7,locSrv,qz +120,7,locSrv,qw +121,2,locSrvZ,tick +122,2,crtp,rxRate +123,2,crtp,txRate +124,7,extrx,thrust +125,7,extrx,roll +126,7,extrx,pitch +127,7,extrx,rollRate +128,7,extrx,pitchRate +129,7,extrx,yawRate +130,7,extrx,zVel +131,1,extrx,AltHold +132,1,extrx,Arm +133,2,extrx_raw,ch0 +134,2,extrx_raw,ch1 +135,2,extrx_raw,ch2 +136,2,extrx_raw,ch3 +137,2,extrx_raw,ch4 +138,2,extrx_raw,ch5 +139,2,extrx_raw,ch6 +140,2,extrx_raw,ch7 +141,7,health,motorVarXM1 +142,7,health,motorVarYM1 +143,7,health,motorVarXM2 +144,7,health,motorVarYM2 +145,7,health,motorVarXM3 +146,7,health,motorVarYM3 +147,7,health,motorVarXM4 +148,7,health,motorVarYM4 +149,1,health,motorPass +150,7,health,batterySag +151,1,health,batteryPass +152,2,health,motorTestCount +153,3,memTst,errCntW +154,2,range,front +155,2,range,back +156,2,range,up +157,2,range,left +158,2,range,right +159,2,range,zrange +160,7,sensfusion6,qw +161,7,sensfusion6,qx +162,7,sensfusion6,qy +163,7,sensfusion6,qz +164,7,sensfusion6,gravityX +165,7,sensfusion6,gravityY +166,7,sensfusion6,gravityZ +167,7,sensfusion6,accZbase +168,1,sensfusion6,isInit +169,1,sensfusion6,isCalibrated +170,7,acc,x +171,7,acc,y +172,7,acc,z +173,7,baro,asl +174,7,baro,temp +175,7,baro,pressure +176,5,controller,ctr_yaw +177,7,controller,cmd_thrust +178,7,controller,cmd_roll +179,7,controller,cmd_pitch +180,7,controller,cmd_yaw +181,7,controller,r_roll +182,7,controller,r_pitch +183,7,controller,r_yaw +184,7,controller,accelz +185,7,controller,actuatorThrust +186,7,controller,roll +187,7,controller,pitch +188,7,controller,yaw +189,7,controller,rollRate +190,7,controller,pitchRate +191,7,controller,yawRate +192,7,ctrltarget,x +193,7,ctrltarget,y +194,7,ctrltarget,z +195,7,ctrltarget,vx +196,7,ctrltarget,vy +197,7,ctrltarget,vz +198,7,ctrltarget,ax +199,7,ctrltarget,ay +200,7,ctrltarget,az +201,7,ctrltarget,roll +202,7,ctrltarget,pitch +203,7,ctrltarget,yaw +204,5,ctrltargetZ,x +205,5,ctrltargetZ,y +206,5,ctrltargetZ,z +207,5,ctrltargetZ,vx +208,5,ctrltargetZ,vy +209,5,ctrltargetZ,vz +210,5,ctrltargetZ,ax +211,5,ctrltargetZ,ay +212,5,ctrltargetZ,az +213,7,mag,x +214,7,mag,y +215,7,mag,z +216,7,stabilizer,roll +217,7,stabilizer,pitch +218,7,stabilizer,yaw +219,7,stabilizer,thrust +220,7,stabilizer,rtStab +221,3,stabilizer,intToOut +222,7,stateEstimate,x +223,7,stateEstimate,y +224,7,stateEstimate,z +225,7,stateEstimate,vx +226,7,stateEstimate,vy +227,7,stateEstimate,vz +228,7,stateEstimate,ax +229,7,stateEstimate,ay +230,7,stateEstimate,az +231,7,stateEstimate,roll +232,7,stateEstimate,pitch +233,7,stateEstimate,yaw +234,7,stateEstimate,qx +235,7,stateEstimate,qy +236,7,stateEstimate,qz +237,7,stateEstimate,qw +238,5,stateEstimateZ,x +239,5,stateEstimateZ,y +240,5,stateEstimateZ,z +241,5,stateEstimateZ,vx +242,5,stateEstimateZ,vy +243,5,stateEstimateZ,vz +244,5,stateEstimateZ,ax +245,5,stateEstimateZ,ay +246,5,stateEstimateZ,az +247,3,stateEstimateZ,quat +248,5,stateEstimateZ,rateRoll +249,5,stateEstimateZ,ratePitch +250,5,stateEstimateZ,rateYaw +251,2,supervisor,info +252,1,sys,canfly +253,1,sys,isFlying +254,1,sys,isTumbled +255,4,sys,testLogParam +256,7,tdoaEngine,stRx +257,7,tdoaEngine,stEst +258,7,tdoaEngine,stTime +259,7,tdoaEngine,stFound +260,7,tdoaEngine,stCc +261,7,tdoaEngine,stHit +262,7,tdoaEngine,stMiss +263,7,tdoaEngine,cc +264,2,tdoaEngine,tof +265,7,tdoaEngine,tdoa +266,7,kalman_pred,predNX +267,7,kalman_pred,predNY +268,7,kalman_pred,measNX +269,7,kalman_pred,measNY +270,1,lighthouse,validAngles +271,7,lighthouse,rawAngle0x +272,7,lighthouse,rawAngle0y +273,7,lighthouse,rawAngle1x +274,7,lighthouse,rawAngle1y +275,7,lighthouse,angle0x +276,7,lighthouse,angle0y +277,7,lighthouse,angle1x +278,7,lighthouse,angle1y +279,7,lighthouse,angle0x_1 +280,7,lighthouse,angle0y_1 +281,7,lighthouse,angle1x_1 +282,7,lighthouse,angle1y_1 +283,7,lighthouse,angle0x_2 +284,7,lighthouse,angle0y_2 +285,7,lighthouse,angle1x_2 +286,7,lighthouse,angle1y_2 +287,7,lighthouse,angle0x_3 +288,7,lighthouse,angle0y_3 +289,7,lighthouse,angle1x_3 +290,7,lighthouse,angle1y_3 +291,7,lighthouse,rawAngle0xlh2 +292,7,lighthouse,rawAngle0ylh2 +293,7,lighthouse,rawAngle1xlh2 +294,7,lighthouse,rawAngle1ylh2 +295,7,lighthouse,angle0x_0lh2 +296,7,lighthouse,angle0y_0lh2 +297,7,lighthouse,angle1x_0lh2 +298,7,lighthouse,angle1y_0lh2 +299,2,lighthouse,width0 +300,2,lighthouse,width1 +301,2,lighthouse,width2 +302,2,lighthouse,width3 +303,1,lighthouse,comSync +304,2,lighthouse,bsAvailable +305,2,lighthouse,bsReceive +306,2,lighthouse,bsActive +307,2,lighthouse,bsCalUd +308,2,lighthouse,bsCalCon +309,1,lighthouse,status +310,7,lighthouse,posRt +311,7,lighthouse,estBs0Rt +312,7,lighthouse,estBs1Rt +313,7,lighthouse,x +314,7,lighthouse,y +315,7,lighthouse,z +316,7,lighthouse,delta +317,2,lighthouse,bsGeoVal +318,2,lighthouse,bsCalVal +319,7,lighthouse,disProb +320,7,pid_attitude,roll_outP +321,7,pid_attitude,roll_outI +322,7,pid_attitude,roll_outD +323,7,pid_attitude,roll_outFF +324,7,pid_attitude,pitch_outP +325,7,pid_attitude,pitch_outI +326,7,pid_attitude,pitch_outD +327,7,pid_attitude,pitch_outFF +328,7,pid_attitude,yaw_outP +329,7,pid_attitude,yaw_outI +330,7,pid_attitude,yaw_outD +331,7,pid_attitude,yaw_outFF +332,7,pid_rate,roll_outP +333,7,pid_rate,roll_outI +334,7,pid_rate,roll_outD +335,7,pid_rate,roll_outFF +336,7,pid_rate,pitch_outP +337,7,pid_rate,pitch_outI +338,7,pid_rate,pitch_outD +339,7,pid_rate,pitch_outFF +340,7,pid_rate,yaw_outP +341,7,pid_rate,yaw_outI +342,7,pid_rate,yaw_outD +343,7,pid_rate,yaw_outFF +344,7,ctrlINDI,cmd_thrust +345,7,ctrlINDI,cmd_roll +346,7,ctrlINDI,cmd_pitch +347,7,ctrlINDI,cmd_yaw +348,7,ctrlINDI,r_roll +349,7,ctrlINDI,r_pitch +350,7,ctrlINDI,r_yaw +351,7,ctrlINDI,u_act_dyn_p +352,7,ctrlINDI,u_act_dyn_q +353,7,ctrlINDI,u_act_dyn_r +354,7,ctrlINDI,du_p +355,7,ctrlINDI,du_q +356,7,ctrlINDI,du_r +357,7,ctrlINDI,ang_accel_ref_p +358,7,ctrlINDI,ang_accel_ref_q +359,7,ctrlINDI,ang_accel_ref_r +360,7,ctrlINDI,rate_d[0] +361,7,ctrlINDI,rate_d[1] +362,7,ctrlINDI,rate_d[2] +363,7,ctrlINDI,uf_p +364,7,ctrlINDI,uf_q +365,7,ctrlINDI,uf_r +366,7,ctrlINDI,Omega_f_p +367,7,ctrlINDI,Omega_f_q +368,7,ctrlINDI,Omega_f_r +369,7,ctrlINDI,n_p +370,7,ctrlINDI,n_q +371,7,ctrlINDI,n_r +372,7,ctrlMel,cmd_thrust +373,7,ctrlMel,cmd_roll +374,7,ctrlMel,cmd_pitch +375,7,ctrlMel,cmd_yaw +376,7,ctrlMel,r_roll +377,7,ctrlMel,r_pitch +378,7,ctrlMel,r_yaw +379,7,ctrlMel,accelz +380,7,ctrlMel,zdx +381,7,ctrlMel,zdy +382,7,ctrlMel,zdz +383,7,ctrlMel,i_err_x +384,7,ctrlMel,i_err_y +385,7,ctrlMel,i_err_z +386,7,posCtrlIndi,posRef_x +387,7,posCtrlIndi,posRef_y +388,7,posCtrlIndi,posRef_z +389,7,posCtrlIndi,velS_x +390,7,posCtrlIndi,velS_y +391,7,posCtrlIndi,velS_z +392,7,posCtrlIndi,velRef_x +393,7,posCtrlIndi,velRef_y +394,7,posCtrlIndi,velRef_z +395,7,posCtrlIndi,angS_roll +396,7,posCtrlIndi,angS_pitch +397,7,posCtrlIndi,angS_yaw +398,7,posCtrlIndi,angF_roll +399,7,posCtrlIndi,angF_pitch +400,7,posCtrlIndi,angF_yaw +401,7,posCtrlIndi,accRef_x +402,7,posCtrlIndi,accRef_y +403,7,posCtrlIndi,accRef_z +404,7,posCtrlIndi,accS_x +405,7,posCtrlIndi,accS_y +406,7,posCtrlIndi,accS_z +407,7,posCtrlIndi,accF_x +408,7,posCtrlIndi,accF_y +409,7,posCtrlIndi,accF_z +410,7,posCtrlIndi,accFT_x +411,7,posCtrlIndi,accFT_y +412,7,posCtrlIndi,accFT_z +413,7,posCtrlIndi,accErr_x +414,7,posCtrlIndi,accErr_y +415,7,posCtrlIndi,accErr_z +416,7,posCtrlIndi,phi_tilde +417,7,posCtrlIndi,theta_tilde +418,7,posCtrlIndi,T_tilde +419,7,posCtrlIndi,T_inner +420,7,posCtrlIndi,T_inner_f +421,7,posCtrlIndi,T_incremented +422,7,posCtrlIndi,cmd_phi +423,7,posCtrlIndi,cmd_theta +424,7,posCtl,targetVX +425,7,posCtl,targetVY +426,7,posCtl,targetVZ +427,7,posCtl,targetX +428,7,posCtl,targetY +429,7,posCtl,targetZ +430,7,posCtl,bodyVX +431,7,posCtl,bodyVY +432,7,posCtl,bodyX +433,7,posCtl,bodyY +434,7,posCtl,Xp +435,7,posCtl,Xi +436,7,posCtl,Xd +437,7,posCtl,Xff +438,7,posCtl,Yp +439,7,posCtl,Yi +440,7,posCtl,Yd +441,7,posCtl,Yff +442,7,posCtl,Zp +443,7,posCtl,Zi +444,7,posCtl,Zd +445,7,posCtl,Zff +446,7,posCtl,VXp +447,7,posCtl,VXi +448,7,posCtl,VXd +449,7,posCtl,VXff +450,7,posCtl,VYp +451,7,posCtl,VYi +452,7,posCtl,VYd +453,7,posCtl,VYff +454,7,posCtl,VZp +455,7,posCtl,VZi +456,7,posCtl,VZd +457,7,posCtl,VZff +458,7,ctrlLee,KR_x +459,7,ctrlLee,KR_y +460,7,ctrlLee,KR_z +461,7,ctrlLee,Kw_x +462,7,ctrlLee,Kw_y +463,7,ctrlLee,Kw_z +464,7,ctrlLee,Kpos_Px +465,7,ctrlLee,Kpos_Py +466,7,ctrlLee,Kpos_Pz +467,7,ctrlLee,Kpos_Dx +468,7,ctrlLee,Kpos_Dy +469,7,ctrlLee,Kpos_Dz +470,7,ctrlLee,thrustSi +471,7,ctrlLee,torquex +472,7,ctrlLee,torquey +473,7,ctrlLee,torquez +474,7,ctrlLee,rpyx +475,7,ctrlLee,rpyy +476,7,ctrlLee,rpyz +477,7,ctrlLee,rpydx +478,7,ctrlLee,rpydy +479,7,ctrlLee,rpydz +480,7,ctrlLee,error_posx +481,7,ctrlLee,error_posy +482,7,ctrlLee,error_posz +483,7,ctrlLee,error_velx +484,7,ctrlLee,error_vely +485,7,ctrlLee,error_velz +486,7,ctrlLee,omegax +487,7,ctrlLee,omegay +488,7,ctrlLee,omegaz +489,7,ctrlLee,omegarx +490,7,ctrlLee,omegary +491,7,ctrlLee,omegarz +492,7,kalman,stateX +493,7,kalman,stateY +494,7,kalman,stateZ +495,7,kalman,statePX +496,7,kalman,statePY +497,7,kalman,statePZ +498,7,kalman,stateD0 +499,7,kalman,stateD1 +500,7,kalman,stateD2 +501,7,kalman,varX +502,7,kalman,varY +503,7,kalman,varZ +504,7,kalman,varPX +505,7,kalman,varPY +506,7,kalman,varPZ +507,7,kalman,varD0 +508,7,kalman,varD1 +509,7,kalman,varD2 +510,7,kalman,q0 +511,7,kalman,q1 +512,7,kalman,q2 +513,7,kalman,q3 +514,7,kalman,rtUpdate +515,7,kalman,rtPred +516,7,kalman,rtFinal +517,6,outlierf,lhWin +518,7,estimator,rtApnd +519,7,estimator,rtRej +520,7,posEstAlt,estimatedZ +521,7,posEstAlt,estVZ +522,7,posEstAlt,velocityZ +523,1,DTR_P2P,rx_state +524,1,DTR_P2P,tx_state diff --git a/trackers/kr_trackers_manager/params3679191350.csv b/trackers/kr_trackers_manager/params3679191350.csv new file mode 100644 index 00000000..9e807627 --- /dev/null +++ b/trackers/kr_trackers_manager/params3679191350.csv @@ -0,0 +1,311 @@ +id,type,readonly,group,name +0,8,0,radiotest,channel +1,0,0,radiotest,power +2,8,0,radiotest,contwave +3,8,0,activeMarker,front +4,8,0,activeMarker,back +5,8,0,activeMarker,left +6,8,0,activeMarker,right +7,8,0,activeMarker,mode +8,8,0,activeMarker,poll +9,8,1,deck,bcActiveMarker +10,8,1,deck,bcAI +11,8,1,deck,bcBuzzer +12,8,1,deck,bcFlow +13,8,1,deck,bcFlow2 +14,8,1,deck,bcLedRing +15,8,1,deck,bcLighthouse4 +16,8,1,deck,bcDWM1000 +17,8,1,deck,bcLoco +18,8,1,deck,bcMultiranger +19,8,1,deck,bcOA +20,8,1,deck,bcUSD +21,8,1,deck,bcZRanger +22,8,1,deck,bcZRanger2 +23,8,0,motion,disable +24,8,0,motion,adaptive +25,6,0,motion,flowStdFixed +26,8,0,ring,effect +27,10,1,ring,neffect +28,8,0,ring,solidRed +29,8,0,ring,solidGreen +30,8,0,ring,solidBlue +31,8,0,ring,headlightEnable +32,6,0,ring,emptyCharge +33,6,0,ring,fullCharge +34,10,0,ring,fadeColor +35,6,0,ring,fadeTime +36,8,0,system,highlight +37,8,0,system,storageStats +38,8,0,system,storageReformat +39,8,0,system,taskDump +40,0,1,system,selftestPassed +41,8,0,system,assertInfo +42,8,0,system,testLogParam +43,8,0,system,doAssert +44,8,0,loco,mode +45,6,0,tdoa2,stddev +46,6,0,tdoa3,stddev +47,9,0,multiranger,filterMask +48,8,1,usd,canLog +49,8,0,usd,logging +50,8,0,led,bitmask +51,8,0,motorPowerSet,enable +52,9,0,motorPowerSet,m1 +53,9,0,motorPowerSet,m2 +54,9,0,motorPowerSet,m3 +55,9,0,motorPowerSet,m4 +56,6,0,pm,lowVoltage +57,6,0,pm,criticalLowVoltage +58,8,1,imu_sensors,BMP3XX +59,6,0,imu_sensors,imuPhi +60,6,0,imu_sensors,imuTheta +61,6,0,imu_sensors,imuPsi +62,8,1,imu_sensors,AK8963 +63,8,1,imu_sensors,LPS25H +64,8,1,imu_tests,MPU6500 +65,8,1,imu_tests,AK8963 +66,8,1,imu_tests,LPS25H +67,6,0,imu_tests,imuPhi +68,6,0,imu_tests,imuTheta +69,6,0,imu_tests,imuPsi +70,8,0,syslink,probe +71,8,0,usec,reset +72,8,0,colAv,enable +73,6,0,colAv,ellipsoidX +74,6,0,colAv,ellipsoidY +75,6,0,colAv,ellipsoidZ +76,6,0,colAv,bboxMinX +77,6,0,colAv,bboxMinY +78,6,0,colAv,bboxMinZ +79,6,0,colAv,bboxMaxX +80,6,0,colAv,bboxMaxY +81,6,0,colAv,bboxMaxZ +82,6,0,colAv,horizon +83,6,0,colAv,maxSpeed +84,6,0,colAv,sidestepThrsh +85,2,0,colAv,maxPeerLocAge +86,6,0,colAv,vorTol +87,2,0,colAv,vorIters +88,8,0,commander,enHighLevel +89,6,0,cppm,rateRoll +90,6,0,cppm,ratePitch +91,6,0,cppm,angPitch +92,6,0,cppm,angRoll +93,6,0,cppm,rateYaw +94,6,0,hlCommander,vtoff +95,6,0,hlCommander,vland +96,8,0,hlCommander,groupmask +97,8,0,flightmode,althold +98,8,0,flightmode,poshold +99,8,0,flightmode,posSet +100,8,0,flightmode,yawMode +101,8,0,flightmode,stabModeRoll +102,8,0,flightmode,stabModePitch +103,8,0,flightmode,stabModeYaw +104,8,0,locSrv,enRangeStreamFP32 +105,8,0,locSrv,enLhAngleStream +106,6,0,locSrv,extPosStdDev +107,6,0,locSrv,extQuatStdDev +108,9,0,crtpsrv,echoDelay +109,8,0,health,startPropTest +110,8,0,health,startBatTest +111,9,0,health,propTestPWMRatio +112,9,0,health,batTestPWMRatio +113,6,0,kalman,maxPos +114,6,0,kalman,maxVel +115,8,0,kalman,resetEstimation +116,8,0,kalman,robustTdoa +117,8,0,kalman,robustTwr +118,6,0,kalman,pNAcc_xy +119,6,0,kalman,pNAcc_z +120,6,0,kalman,pNVel +121,6,0,kalman,pNPos +122,6,0,kalman,pNAtt +123,6,0,kalman,mNBaro +124,6,0,kalman,mNGyro_rollpitch +125,6,0,kalman,mNGyro_yaw +126,6,0,kalman,initialX +127,6,0,kalman,initialY +128,6,0,kalman,initialZ +129,6,0,kalman,initialYaw +130,8,0,memTst,resetW +131,10,0,powerDist,idleThrust +132,6,0,quadSysId,thrustToTorque +133,6,0,quadSysId,pwmToThrustA +134,6,0,quadSysId,pwmToThrustB +135,6,0,quadSysId,armLength +136,6,0,sensfusion6,kp +137,6,0,sensfusion6,ki +138,6,0,sensfusion6,baseZacc +139,8,0,sound,effect +140,10,1,sound,neffect +141,9,0,sound,freq +142,8,0,stabilizer,estimator +143,8,0,stabilizer,controller +144,8,0,stabilizer,stop +145,8,0,supervisor,infdmp +146,9,0,supervisor,prefltTimeout +147,9,0,supervisor,landedTimeout +148,8,0,supervisor,tmblChckEn +149,9,1,cpu,flash +150,10,1,cpu,id0 +151,10,1,cpu,id1 +152,10,1,cpu,id2 +153,8,0,tdoaEngine,logId +154,8,0,tdoaEngine,logOthrId +155,8,0,tdoaEngine,matchAlgo +156,8,0,lighthouse,method +157,8,0,lighthouse,bsCalibReset +158,8,0,lighthouse,systemType +159,9,1,lighthouse,bsAvailable +160,6,0,lighthouse,sweepStd +161,6,0,lighthouse,sweepStd2 +162,8,0,lighthouse,enLhRawStream +163,9,0,lighthouse,lh2maxRate +164,6,0,pid_attitude,roll_kp +165,6,0,pid_attitude,roll_ki +166,6,0,pid_attitude,roll_kd +167,6,0,pid_attitude,roll_kff +168,6,0,pid_attitude,pitch_kp +169,6,0,pid_attitude,pitch_ki +170,6,0,pid_attitude,pitch_kd +171,6,0,pid_attitude,pitch_kff +172,6,0,pid_attitude,yaw_kp +173,6,0,pid_attitude,yaw_ki +174,6,0,pid_attitude,yaw_kd +175,6,0,pid_attitude,yaw_kff +176,6,0,pid_attitude,yawMaxDelta +177,0,0,pid_attitude,attFiltEn +178,6,0,pid_attitude,attFiltCut +179,6,0,pid_rate,roll_kp +180,6,0,pid_rate,roll_ki +181,6,0,pid_rate,roll_kd +182,6,0,pid_rate,roll_kff +183,6,0,pid_rate,pitch_kp +184,6,0,pid_rate,pitch_ki +185,6,0,pid_rate,pitch_kd +186,6,0,pid_rate,pitch_kff +187,6,0,pid_rate,yaw_kp +188,6,0,pid_rate,yaw_ki +189,6,0,pid_rate,yaw_kd +190,6,0,pid_rate,yaw_kff +191,0,0,pid_rate,rateFiltEn +192,6,0,pid_rate,omxFiltCut +193,6,0,pid_rate,omyFiltCut +194,6,0,pid_rate,omzFiltCut +195,6,0,ctrlINDI,thrust_threshold +196,6,0,ctrlINDI,bound_ctrl_input +197,6,0,ctrlINDI,g1_p +198,6,0,ctrlINDI,g1_q +199,6,0,ctrlINDI,g1_r +200,6,0,ctrlINDI,g2 +201,6,0,ctrlINDI,ref_err_p +202,6,0,ctrlINDI,ref_err_q +203,6,0,ctrlINDI,ref_err_r +204,6,0,ctrlINDI,ref_rate_p +205,6,0,ctrlINDI,ref_rate_q +206,6,0,ctrlINDI,ref_rate_r +207,6,0,ctrlINDI,act_dyn_p +208,6,0,ctrlINDI,act_dyn_q +209,6,0,ctrlINDI,act_dyn_r +210,6,0,ctrlINDI,filt_cutoff +211,6,0,ctrlINDI,filt_cutoff_r +212,8,0,ctrlINDI,outerLoopActive +213,6,0,ctrlMel,kp_xy +214,6,0,ctrlMel,kd_xy +215,6,0,ctrlMel,ki_xy +216,6,0,ctrlMel,i_range_xy +217,6,0,ctrlMel,kp_z +218,6,0,ctrlMel,kd_z +219,6,0,ctrlMel,ki_z +220,6,0,ctrlMel,i_range_z +221,6,0,ctrlMel,mass +222,6,0,ctrlMel,massThrust +223,6,0,ctrlMel,kR_xy +224,6,0,ctrlMel,kR_z +225,6,0,ctrlMel,kw_xy +226,6,0,ctrlMel,kw_z +227,6,0,ctrlMel,ki_m_xy +228,6,0,ctrlMel,ki_m_z +229,6,0,ctrlMel,kd_omega_rp +230,6,0,ctrlMel,i_range_m_xy +231,6,0,ctrlMel,i_range_m_z +232,6,0,ctrlAtt,tau_xy +233,6,0,ctrlAtt,zeta_xy +234,6,0,ctrlAtt,tau_z +235,6,0,ctrlAtt,zeta_z +236,6,0,ctrlAtt,tau_rp +237,6,0,ctrlAtt,mixing_factor +238,6,0,ctrlAtt,coll_fairness +239,6,0,posCtrlIndi,K_xi_x +240,6,0,posCtrlIndi,K_xi_y +241,6,0,posCtrlIndi,K_xi_z +242,6,0,posCtrlIndi,K_dxi_x +243,6,0,posCtrlIndi,K_dxi_y +244,6,0,posCtrlIndi,K_dxi_z +245,6,0,posCtrlIndi,pq_clamping +246,6,0,posCtlPid,xKp +247,6,0,posCtlPid,xKi +248,6,0,posCtlPid,xKd +249,6,0,posCtlPid,xKff +250,6,0,posCtlPid,yKp +251,6,0,posCtlPid,yKi +252,6,0,posCtlPid,yKd +253,6,0,posCtlPid,yKff +254,6,0,posCtlPid,zKp +255,6,0,posCtlPid,zKi +256,6,0,posCtlPid,zKd +257,6,0,posCtlPid,zKff +258,9,0,posCtlPid,thrustBase +259,9,0,posCtlPid,thrustMin +260,6,0,posCtlPid,rLimit +261,6,0,posCtlPid,pLimit +262,6,0,posCtlPid,xVelMax +263,6,0,posCtlPid,yVelMax +264,6,0,posCtlPid,zVelMax +265,6,0,velCtlPid,vxKp +266,6,0,velCtlPid,vxKi +267,6,0,velCtlPid,vxKd +268,6,0,velCtlPid,vxKFF +269,6,0,velCtlPid,vyKp +270,6,0,velCtlPid,vyKi +271,6,0,velCtlPid,vyKd +272,6,0,velCtlPid,vyKFF +273,6,0,velCtlPid,vzKp +274,6,0,velCtlPid,vzKi +275,6,0,velCtlPid,vzKd +276,6,0,velCtlPid,vzKFF +277,6,0,ctrlLee,KR_x +278,6,0,ctrlLee,KR_y +279,6,0,ctrlLee,KR_z +280,6,0,ctrlLee,KI_x +281,6,0,ctrlLee,KI_y +282,6,0,ctrlLee,KI_z +283,6,0,ctrlLee,Kw_x +284,6,0,ctrlLee,Kw_y +285,6,0,ctrlLee,Kw_z +286,6,0,ctrlLee,J_x +287,6,0,ctrlLee,J_y +288,6,0,ctrlLee,J_z +289,6,0,ctrlLee,Kpos_Px +290,6,0,ctrlLee,Kpos_Py +291,6,0,ctrlLee,Kpos_Pz +292,6,0,ctrlLee,Kpos_P_limit +293,6,0,ctrlLee,Kpos_Dx +294,6,0,ctrlLee,Kpos_Dy +295,6,0,ctrlLee,Kpos_Dz +296,6,0,ctrlLee,Kpos_D_limit +297,6,0,ctrlLee,Kpos_Ix +298,6,0,ctrlLee,Kpos_Iy +299,6,0,ctrlLee,Kpos_Iz +300,6,0,ctrlLee,Kpos_I_limit +301,6,0,ctrlLee,mass +302,6,0,posEstAlt,estAlphaAsl +303,6,0,posEstAlt,estAlphaZr +304,6,0,posEstAlt,velFactor +305,6,0,posEstAlt,velZAlpha +306,6,0,posEstAlt,vAccDeadband +307,10,1,firmware,revision0 +308,9,1,firmware,revision1 +309,8,1,firmware,modified From c9426404124111ecb0bf07046a679030a233cfe2 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Tue, 25 Mar 2025 15:29:09 -0400 Subject: [PATCH 33/64] save before switching drones --- kr_mav_manager/src/manager.cpp | 139 +++++++------ .../src/line_tracker_distance_server.cpp | 183 +++++++++++++----- 2 files changed, 214 insertions(+), 108 deletions(-) diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index bbaa325d..f06a2809 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -278,69 +278,94 @@ bool MAVManager::takeoff() RCLCPP_INFO(this->get_logger(), "Initiating launch sequence..."); - auto goal = LineTracker::Goal(); - goal.z = takeoff_height_; - goal.relative = true; - // Create goal options with lambda callbacks - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - // Goal response callback - send_goal_options.goal_response_callback = - [this](const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle) { - if (!goal_handle) { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } else { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - } - }; + auto goal = std::make_shared(); + goal->z = takeoff_height_; + goal->relative = true; // Result callback - send_goal_options.result_callback = - [this](const rclcpp_action::ClientGoalHandle::WrappedResult & result) { - RCLCPP_INFO(this->get_logger(), " IN LAMBDA"); - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - RCLCPP_INFO(this->get_logger(), "Takeoff successful"); - if(this->transition(line_tracker_distance)) - { - status_ = FLYING; - return true; - } - else - return false; - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Takeoff aborted"); - break; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Takeoff canceled"); - break; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - break; - } - }; - line_tracker_distance_client_->async_send_goal(goal, send_goal_options); - return true; - - // auto future = std::async(std::launch::async, &MAVManager::send_takeoff_request, this, goal); - // if(future.get()) + // send_goal_options.result_callback = + // [this](const rclcpp_action::ClientGoalHandle::WrappedResult & result) { + // RCLCPP_INFO(this->get_logger(), " IN LAMBDA"); + // switch (result.code) { + // case rclcpp_action::ResultCode::SUCCEEDED: + // RCLCPP_INFO(this->get_logger(), "Takeoff successful"); + // if(this->transition(line_tracker_distance)) + // { + // status_ = FLYING; + // return true; + // } + // else + // return false; + // break; + // case rclcpp_action::ResultCode::ABORTED: + // RCLCPP_ERROR(this->get_logger(), "Takeoff aborted"); + // break; + // case rclcpp_action::ResultCode::CANCELED: + // RCLCPP_ERROR(this->get_logger(), "Takeoff canceled"); + // break; + // default: + // RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + // break; + // } + // }; + /////// + // line_tracker_distance_client_->async_send_goal(goal, send_goal_options); + // std::this_thread::sleep_for(std::chrono::seconds(1)); + // if(this->transition(line_tracker_distance)) // { - // RCLCPP_INFO(this->get_logger(), "got trueeeeeee"); + // status_ = FLYING; + // return true; // } - // return false; + // else + // return false; + // return true; + + auto future = std::async(std::launch::async, &MAVManager::send_takeoff_request, this, goal); + if(future.get()) + { + RCLCPP_INFO(this->get_logger(), "got trueeeeeee"); + if(this->transition(line_tracker_distance)) + { + status_ = FLYING; + return true; + } + else + return false; + } } -// bool MAVManager::send_takeoff_request(std::shared_ptr goal) -// { -// RCLCPP_INFO(this->get_logger(), "IN SEND TAKEOFF REQ"); -// auto future = line_tracker_distance_client_->async_send_goal(*goal); -// auto status = future.wait_for(std::chrono::seconds(5)); -// if (status == std::future_status::timeout) { -// RCLCPP_ERROR(this->get_logger(), "Future timed out waiting for goal response"); -// return false; -// } -// } +bool MAVManager::send_takeoff_request(std::shared_ptr goal) +{ + + // // Create goal options with lambda callbacks + // auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + // + // // Goal response callback + // send_goal_options.goal_response_callback = + // [this](const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle) { + // if (!goal_handle) { + // RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + // } else { + // RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + // std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // return true; + // } + // }; + RCLCPP_INFO(this->get_logger(), "IN SEND TAKEOFF REQ"); + auto future = line_tracker_distance_client_->async_send_goal(*goal); + try + { + RCLCPP_INFO(this->get_logger(), "IN TRY"); + auto response = future.get(); + return true; + } + catch(const std::exception &e) + { + RCLCPP_INFO(this->get_logger(), "IN CATCH"); + RCLCPP_WARN(this->get_logger(), "Transition callback failed"); + } + return false; +} bool MAVManager::set_mass(float m) { diff --git a/trackers/kr_trackers/src/line_tracker_distance_server.cpp b/trackers/kr_trackers/src/line_tracker_distance_server.cpp index 8baedb76..cab3f3d1 100644 --- a/trackers/kr_trackers/src/line_tracker_distance_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_distance_server.cpp @@ -301,96 +301,177 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co return cmd; } +// rclcpp_action::GoalResponse LineTrackerDistance::goal_callback(const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) +// { +// (void)uuid; +// (void)goal; +// // If another goal is already active, cancel that goal +// // and track this one instead +// std::lock_guard lock(mutex_); +// RCLCPP_INFO_STREAM(logger_, "current_goal_handle_: " << current_goal_handle_); +// +// if (current_goal_handle_ == 0) { +// RCLCPP_INFO_STREAM(logger_, "Initialization of line tracker detected"); +// goal_set_ = false; +// return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +// } +// +// if(current_goal_handle_ && current_goal_handle_->is_active()) +// { +// RCLCPP_INFO(logger_, "LineTrackerDistance goal (%2.2f, %2.2f, %2.2f) preempted.", goal_(0), goal_(1), goal_(2)); +// preempted_goal_id_ = current_goal_handle_->get_goal_id(); +// preempt_requested_ = true; +// +// goal_ = pos_; +// goal_set_ = false; +// goal_reached_ = false; +// } +// +// RCLCPP_INFO(logger_, "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); +// return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +// } +// +// rclcpp_action::CancelResponse LineTrackerDistance::cancel_callback(const std::shared_ptr goal_handle) +// { +// RCLCPP_INFO(logger_, "Received Cancel Request."); +// (void)goal_handle; +// +// goal_ = pos_; +// goal_set_ = false; +// goal_reached_ = false; +// +// return rclcpp_action::CancelResponse::ACCEPT; +// } +// +// rclcpp_action::ResultCode LineTrackerDistance::handle_accepted_callback(const std::shared_ptr goal_handle) +// { +// std::lock_guard lock(mutex_); +// +// if(current_goal_handle_ && preempt_requested_) +// { +// if(current_goal_handle_->get_goal_id() == preempted_goal_id_) +// { +// RCLCPP_INFO(logger_, "LineTrackerDistance going to goal (%2.2f, %2.2f, %2.2f) aborted.", goal_(0), goal_(1), goal_(2)); +// current_goal_handle_->abort(result_); +// preempt_requested_ = false; +// } +// } +// +// // Pointer to the goal received +// RCLCPP_INFO_STREAM(logger_, "New Goal: " << goal_handle); +// current_goal_handle_ = goal_handle; +// +// auto goal = goal_handle->get_goal(); +// goal_(0) = goal->x; +// goal_(1) = goal->y; +// goal_(2) = goal->z; +// +// if(goal->relative) +// goal_ += ICs_.pos(); +// +// if(goal->v_des > 0) +// v_des_ = goal->v_des; +// else +// v_des_ = default_v_des_; +// +// if(goal->a_des > 0) +// a_des_ = goal->a_des; +// else +// a_des_ = default_a_des_; +// +// start_ = pos_; +// start_yaw_ = yaw_; +// +// current_traj_length_ = 0.0; +// current_traj_duration_ = 0.0; +// +// goal_set_ = true; +// goal_reached_ = false; +// RCLCPP_INFO_STREAM(logger_, "Out of HAC: "); +// // auto result = std::make_shared(); +// // goal_handle->succeed(result); +// // return rclcpp_action::ResultCode::SUCCEEDED; +// } + rclcpp_action::GoalResponse LineTrackerDistance::goal_callback(const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) { - (void)uuid; - (void)goal; - // If another goal is already active, cancel that goal - // and track this one instead std::lock_guard lock(mutex_); RCLCPP_INFO_STREAM(logger_, "current_goal_handle_: " << current_goal_handle_); - - if (current_goal_handle_ == 0) { + + if (current_goal_handle_ == 0) { RCLCPP_INFO_STREAM(logger_, "Initialization of line tracker detected"); goal_set_ = false; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } - - if(current_goal_handle_ && current_goal_handle_->is_active()) - { + + if(current_goal_handle_ && current_goal_handle_->is_active()) { RCLCPP_INFO(logger_, "LineTrackerDistance goal (%2.2f, %2.2f, %2.2f) preempted.", goal_(0), goal_(1), goal_(2)); preempted_goal_id_ = current_goal_handle_->get_goal_id(); preempt_requested_ = true; - goal_ = pos_; goal_set_ = false; goal_reached_ = false; } - - RCLCPP_INFO(logger_, "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse LineTrackerDistance::cancel_callback(const std::shared_ptr goal_handle) -{ - RCLCPP_INFO(logger_, "Received Cancel Request."); - (void)goal_handle; - - goal_ = pos_; - goal_set_ = false; - goal_reached_ = false; - - return rclcpp_action::CancelResponse::ACCEPT; -} - -rclcpp_action::ResultCode LineTrackerDistance::handle_accepted_callback(const std::shared_ptr goal_handle) -{ - std::lock_guard lock(mutex_); - - if(current_goal_handle_ && preempt_requested_) - { - if(current_goal_handle_->get_goal_id() == preempted_goal_id_) - { + + // Moving the functionality from handle_accepted_callback to here + if(current_goal_handle_ && preempt_requested_) { + if(current_goal_handle_->get_goal_id() == preempted_goal_id_) { RCLCPP_INFO(logger_, "LineTrackerDistance going to goal (%2.2f, %2.2f, %2.2f) aborted.", goal_(0), goal_(1), goal_(2)); current_goal_handle_->abort(result_); preempt_requested_ = false; } } - - // Pointer to the goal received - RCLCPP_INFO_STREAM(logger_, "New Goal: " << goal_handle); - current_goal_handle_ = goal_handle; - - auto goal = goal_handle->get_goal(); + + // Set up the goal parameters goal_(0) = goal->x; goal_(1) = goal->y; goal_(2) = goal->z; - + if(goal->relative) goal_ += ICs_.pos(); - + if(goal->v_des > 0) v_des_ = goal->v_des; else v_des_ = default_v_des_; - + if(goal->a_des > 0) a_des_ = goal->a_des; else a_des_ = default_a_des_; - + start_ = pos_; start_yaw_ = yaw_; - current_traj_length_ = 0.0; current_traj_duration_ = 0.0; - goal_set_ = true; goal_reached_ = false; + + RCLCPP_INFO(logger_, "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse LineTrackerDistance::cancel_callback(const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(logger_, "Received Cancel Request."); + (void)goal_handle; + goal_ = pos_; + goal_set_ = false; + goal_reached_ = false; + return rclcpp_action::CancelResponse::ACCEPT; +} + +rclcpp_action::ResultCode LineTrackerDistance::handle_accepted_callback(const std::shared_ptr goal_handle) +{ + std::lock_guard lock(mutex_); + + // Pointer to the goal received + RCLCPP_INFO_STREAM(logger_, "New Goal: " << goal_handle); + current_goal_handle_ = goal_handle; + RCLCPP_INFO_STREAM(logger_, "Out of HAC: "); - auto result = std::make_shared(); - goal_handle->succeed(result); - return rclcpp_action::ResultCode::SUCCEEDED; + // auto result = std::make_shared(); + // goal_handle->succeed(result); + // return rclcpp_action::ResultCode::SUCCEEDED; } uint8_t LineTrackerDistance::status() From b2ee362830d39aa2dad82fae5d148bdef79c70d6 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Tue, 25 Mar 2025 18:29:47 -0400 Subject: [PATCH 34/64] drone now takes off on motors on --- .../src/so3cmd_to_crazyflie_component.cpp | 2 +- .../src/so3_control_component.cpp | 2 +- kr_mav_manager/src/manager.cpp | 107 +++++++----------- .../src/line_tracker_distance_server.cpp | 60 +++++++--- trackers/kr_trackers/src/null_tracker.cpp | 1 + .../launch/example.launch.py | 2 +- 6 files changed, 89 insertions(+), 85 deletions(-) diff --git a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp index 4f4d02ad..a1206693 100644 --- a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp +++ b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp @@ -256,7 +256,7 @@ SO3CmdToCrazyflie::SO3CmdToCrazyflie(const rclcpp::NodeOptions &options) auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); so3_cmd_sub_ = this->create_subscription( - "/cf3/so3_cmd", qos2, std::bind(&SO3CmdToCrazyflie::so3_cmd_callback, this, std::placeholders::_1)); + "/cf3/so3_controller/so3_cmd", qos2, std::bind(&SO3CmdToCrazyflie::so3_cmd_callback, this, std::placeholders::_1)); odom_sub_ = this->create_subscription( "/cf3/odom", qos1, std::bind(&SO3CmdToCrazyflie::odom_callback, this, std::placeholders::_1)); diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index be3f1dc1..58ee7e98 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -638,7 +638,7 @@ SO3ControlComponent::SO3ControlComponent(const rclcpp::NodeOptions &options) odom_sub_ = this->create_subscription( "/cf3/odom", qos1, std::bind(&SO3ControlComponent::odom_callback, this, std::placeholders::_1)); position_cmd_sub_ = this->create_subscription( - "/cf3/position_cmd", qos1, std::bind(&SO3ControlComponent::position_cmd_callback, this, std::placeholders::_1)); + "/cf3/trackers_manager/cmd", qos1, std::bind(&SO3ControlComponent::position_cmd_callback, this, std::placeholders::_1)); enable_motors_sub_ = this->create_subscription( "/cf3/motors", qos2, std::bind(&SO3ControlComponent::enable_motors_callback, this, std::placeholders::_1)); corrections_sub_ = this->create_subscription( diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index f06a2809..c60e4184 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -51,7 +51,7 @@ MAVManager::MAVManager() this->declare_parameter("max_attitude_angle", rclcpp::PARAMETER_DOUBLE); this->declare_parameter("mass", rclcpp::PARAMETER_DOUBLE); this->declare_parameter("odom_timeout", 0.1f); - this->declare_parameter("takeoff_height", 0.1); + this->declare_parameter("takeoff_height", 0.5); this->declare_parameter("robot", "cf3"); std::string robot_name = this->get_parameter("robot").as_string(); @@ -102,9 +102,9 @@ MAVManager::MAVManager() // Publishers pub_motors_ = this->create_publisher("motors", 10); pub_estop_ = this->create_publisher("estop", 10); - pub_so3_command_ = this->create_publisher("so3_cmd", 10); + pub_so3_command_ = this->create_publisher("/cf3/so3_controller/so3_cmd", 10); pub_trpy_command_ = this->create_publisher("trpy_cmd", 10); - pub_position_command_ = this->create_publisher("position_cmd", 10); + pub_position_command_ = this->create_publisher("/cf3/trackers_manager/cmd", 10); pub_status_ = this->create_publisher("~/status", 10); pub_goal_velocity_ = this->create_publisher("trackers_manager/velocity_tracker/goal", 10); @@ -278,79 +278,52 @@ bool MAVManager::takeoff() RCLCPP_INFO(this->get_logger(), "Initiating launch sequence..."); - auto goal = std::make_shared(); - goal->z = takeoff_height_; - goal->relative = true; + auto goal = LineTracker::Goal(); + goal.z = takeoff_height_; + goal.relative = true; - // Result callback - // send_goal_options.result_callback = - // [this](const rclcpp_action::ClientGoalHandle::WrappedResult & result) { - // RCLCPP_INFO(this->get_logger(), " IN LAMBDA"); - // switch (result.code) { - // case rclcpp_action::ResultCode::SUCCEEDED: - // RCLCPP_INFO(this->get_logger(), "Takeoff successful"); - // if(this->transition(line_tracker_distance)) - // { - // status_ = FLYING; - // return true; - // } - // else - // return false; - // break; - // case rclcpp_action::ResultCode::ABORTED: - // RCLCPP_ERROR(this->get_logger(), "Takeoff aborted"); - // break; - // case rclcpp_action::ResultCode::CANCELED: - // RCLCPP_ERROR(this->get_logger(), "Takeoff canceled"); - // break; - // default: - // RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - // break; - // } - // }; - /////// - // line_tracker_distance_client_->async_send_goal(goal, send_goal_options); - // std::this_thread::sleep_for(std::chrono::seconds(1)); - // if(this->transition(line_tracker_distance)) - // { - // status_ = FLYING; - // return true; - // } - // else - // return false; - // return true; - - auto future = std::async(std::launch::async, &MAVManager::send_takeoff_request, this, goal); - if(future.get()) + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // Goal response callback + send_goal_options.goal_response_callback = + [this](const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle) { + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + return true; + } + }; + line_tracker_distance_client_->async_send_goal(goal, send_goal_options); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if(this->transition(line_tracker_distance)) { - RCLCPP_INFO(this->get_logger(), "got trueeeeeee"); - if(this->transition(line_tracker_distance)) - { - status_ = FLYING; - return true; - } - else - return false; + status_ = FLYING; + return true; } + else + return false; + return true; + + // auto future = std::async(std::launch::async, &MAVManager::send_takeoff_request, this, goal); + // if(future.get()) + // { + // RCLCPP_INFO(this->get_logger(), "got trueeeeeee"); + // if(this->transition(line_tracker_distance)) + // { + // status_ = FLYING; + // return true; + // } + // else + // return false; + // } } bool MAVManager::send_takeoff_request(std::shared_ptr goal) { // // Create goal options with lambda callbacks - // auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - // - // // Goal response callback - // send_goal_options.goal_response_callback = - // [this](const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle) { - // if (!goal_handle) { - // RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - // } else { - // RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - // std::this_thread::sleep_for(std::chrono::milliseconds(100)); - // return true; - // } - // }; RCLCPP_INFO(this->get_logger(), "IN SEND TAKEOFF REQ"); auto future = line_tracker_distance_client_->async_send_goal(*goal); try diff --git a/trackers/kr_trackers/src/line_tracker_distance_server.cpp b/trackers/kr_trackers/src/line_tracker_distance_server.cpp index cab3f3d1..d2c6150b 100644 --- a/trackers/kr_trackers/src/line_tracker_distance_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_distance_server.cpp @@ -33,7 +33,7 @@ class LineTrackerDistance : public kr_trackers_manager::Tracker 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); - rclcpp_action::ResultCode handle_accepted_callback(const std::shared_ptr goal_handle); + void handle_accepted_callback(const std::shared_ptr goal_handle); rclcpp_action::Server::SharedPtr tracker_server_; rclcpp::CallbackGroup::SharedPtr cb_group_; @@ -119,6 +119,7 @@ bool LineTrackerDistance::Activate(const kr_mav_msgs::msg::PositionCommand::Cons return active_; } } + RCLCPP_INFO(logger_, "In Activate 3"); // Set start and start_yaw here so that even if the goal was sent at a // different position, we still use the current position as start @@ -130,6 +131,7 @@ bool LineTrackerDistance::Activate(const kr_mav_msgs::msg::PositionCommand::Cons active_ = true; } + RCLCPP_INFO(logger_, "In Activate 4"); return active_; } @@ -187,28 +189,37 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co result->duration = current_traj_duration_; result->length = current_traj_length_; result_ = result; + RCLCPP_INFO_STREAM(logger_, "goal_reached_" << goal_reached_); + RCLCPP_INFO(logger_, "UPDATE 1"); - if(current_goal_handle_ && current_goal_handle_->is_canceling()) - { - RCLCPP_INFO(logger_, "LineTrackerDistance going to goal (%2.2f, %2.2f, %2.2f) canceled.", goal_(0), goal_(1), goal_(2)); - - current_goal_handle_->canceled(result); - goal_ = pos_; - goal_set_ = false; - goal_reached_ = false; - return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); + if (current_goal_handle_) { + if(current_goal_handle_ && current_goal_handle_->is_canceling()) + { + RCLCPP_INFO(logger_, "LineTrackerDistance going to goal (%2.2f, %2.2f, %2.2f) canceled.", goal_(0), goal_(1), goal_(2)); + + current_goal_handle_->canceled(result); + goal_ = pos_; + goal_set_ = false; + goal_reached_ = false; + return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); + } } + RCLCPP_INFO(logger_, "UPDATE 2"); auto cmd = std::make_shared(); cmd->header.stamp = clock_->now(); cmd->header.frame_id = msg->header.frame_id; cmd->yaw = start_yaw_; + RCLCPP_INFO(logger_, "UPDATE 3"); if(goal_reached_) { - if(current_goal_handle_->is_active()) - { - RCLCPP_ERROR(logger_, "LineTrackerDistance::update: Action server not completed.\n"); + // RCLCPP_INFO_STREAM(logger_, "current_goal_handle: "<< current_goal_handle_); + if (current_goal_handle_) { + if(current_goal_handle_->is_active()) + { + RCLCPP_ERROR(logger_, "LineTrackerDistance::update: Action server not completed.\n"); + } } cmd->position.x = goal_(0), cmd->position.y = goal_(1), cmd->position.z = goal_(2); @@ -219,6 +230,7 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co return cmd; } + RCLCPP_INFO(logger_, "UPDATE 4"); const Eigen::Vector3f dir = (goal_ - start_).normalized(); const float total_dist = (goal_ - start_).norm(); const float d = (pos_ - start_).dot(dir); @@ -229,8 +241,12 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co Eigen::Vector3f x(pos_), v(Eigen::Vector3f::Zero()), a(Eigen::Vector3f::Zero()); + RCLCPP_INFO_STREAM(logger_, "pos_: " << pos_); + RCLCPP_INFO_STREAM(logger_, "goal_: " << goal_); + if((pos_ - goal_).norm() <= epsilon_) // Reached goal { + RCLCPP_INFO(logger_, "UPDATE 5"); // Send success message and reset the length and duration variables result->duration = current_traj_duration_; result->length = current_traj_length_; @@ -253,6 +269,7 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co } else if(d > total_dist) // Overshoot { + RCLCPP_INFO(logger_, "UPDATE 6"); RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Overshoot"); a = -a_des_ * dir; v = Eigen::Vector3f::Zero(); @@ -260,6 +277,7 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co } else if(d >= (total_dist - ramp_dist) && d <= total_dist) // Decelerate { + RCLCPP_INFO(logger_, "UPDATE 7"); RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Decelerate"); a = -a_des_ * dir; v = std::sqrt(2 * a_des_ * (total_dist - d)) * dir; @@ -267,6 +285,7 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co } else if(d > ramp_dist && d < total_dist - ramp_dist) // Constant velocity { + RCLCPP_INFO(logger_, "UPDATE 8"); RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Constant velocity"); a = Eigen::Vector3f::Zero(); v = v_max * dir; @@ -274,6 +293,7 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co } else if(d >= 0 && d <= ramp_dist) // Accelerate { + RCLCPP_INFO(logger_, "UPDATE 9"); RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Accelerate"); a = a_des_ * dir; v = std::sqrt(2 * a_des_ * d) * dir; @@ -281,6 +301,7 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co } else if(d < 0) { + RCLCPP_INFO(logger_, "UPDATE 10"); RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Undershoot"); a = a_des_ * dir; v = Eigen::Vector3f::Zero(); @@ -293,11 +314,13 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co if(!goal_reached_) { + RCLCPP_INFO(logger_, "UPDATE 11"); auto feedback = std::make_shared(); feedback->distance_from_goal = (pos_ - goal_).norm(); current_goal_handle_->publish_feedback(feedback); } + RCLCPP_INFO(logger_, "UPDATE 12"); return cmd; } @@ -460,7 +483,7 @@ rclcpp_action::CancelResponse LineTrackerDistance::cancel_callback(const std::sh return rclcpp_action::CancelResponse::ACCEPT; } -rclcpp_action::ResultCode LineTrackerDistance::handle_accepted_callback(const std::shared_ptr goal_handle) +void LineTrackerDistance::handle_accepted_callback(const std::shared_ptr goal_handle) { std::lock_guard lock(mutex_); @@ -478,7 +501,14 @@ uint8_t LineTrackerDistance::status() { std::lock_guard lock(mutex_); - return current_goal_handle_->is_active() ? static_cast(kr_tracker_msgs::msg::TrackerStatus::ACTIVE) : static_cast(kr_tracker_msgs::msg::TrackerStatus::SUCCEEDED); + if (!current_goal_handle_) { + return static_cast(kr_tracker_msgs::msg::TrackerStatus::SUCCEEDED); + } + + return current_goal_handle_->is_active() ? + static_cast(kr_tracker_msgs::msg::TrackerStatus::ACTIVE) : + static_cast(kr_tracker_msgs::msg::TrackerStatus::SUCCEEDED); + } #include "pluginlib/class_list_macros.hpp" diff --git a/trackers/kr_trackers/src/null_tracker.cpp b/trackers/kr_trackers/src/null_tracker.cpp index a1d2cf62..95bd8592 100644 --- a/trackers/kr_trackers/src/null_tracker.cpp +++ b/trackers/kr_trackers/src/null_tracker.cpp @@ -32,6 +32,7 @@ void NullTracker::Deactivate(void) {} kr_mav_msgs::msg::PositionCommand::ConstSharedPtr NullTracker::update(const nav_msgs::msg::Odometry::SharedPtr msg) { // Return a null message (will not publish the position command) + return std::make_shared(); } diff --git a/trackers/kr_trackers_manager/launch/example.launch.py b/trackers/kr_trackers_manager/launch/example.launch.py index 5d27d35d..99dae6ea 100644 --- a/trackers/kr_trackers_manager/launch/example.launch.py +++ b/trackers/kr_trackers_manager/launch/example.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): namespace=LaunchConfiguration('namespace'), package="rclcpp_components", executable="component_container_mt", - # prefix=['xterm -e gdb --args'], + #prefix=['xterm -e gdb --args'], composable_node_descriptions= [ ComposableNode( package="kr_trackers_manager", From fe61e7a7316c7890ad4a0a2e240332e6a8c61ef1 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Tue, 25 Mar 2025 19:05:32 -0400 Subject: [PATCH 35/64] WE TOOK OFF --- kr_mav_controllers/src/SO3Control.cpp | 2 ++ kr_mav_controllers/src/so3_control_component.cpp | 6 ++++++ trackers/kr_trackers/src/null_tracker.cpp | 3 ++- 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/kr_mav_controllers/src/SO3Control.cpp b/kr_mav_controllers/src/SO3Control.cpp index 861b45e1..04e98b62 100644 --- a/kr_mav_controllers/src/SO3Control.cpp +++ b/kr_mav_controllers/src/SO3Control.cpp @@ -1,4 +1,5 @@ #include "kr_mav_controllers/SO3Control.hpp" +#include SO3Control::SO3Control() : mass_(0.5), @@ -56,6 +57,7 @@ void SO3Control::calculateControl(const Eigen::Vector3f &des_pos, const Eigen::V const float des_yaw_dot, const Eigen::Vector3f &kx, const Eigen::Vector3f &kv, const Eigen::Vector3f &ki, const Eigen::Vector3f &ki_b) { + const Eigen::Vector3f e_pos = des_pos - pos_; const Eigen::Vector3f e_vel = des_vel - vel_; diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index 58ee7e98..ab6516b0 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -64,6 +64,12 @@ void SO3ControlComponent::publishSO3Command() kib = config_kib_; } + // RCLCPP_INFO_STREAM(this->get_logger(), "des_pos_: " << des_pos_); + // RCLCPP_INFO_STREAM(this->get_logger(), "des_vel_: " << des_vel_); + // 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_); + controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_jrk_, des_yaw_, des_yaw_dot_, kx_, kv_, ki, kib); const Eigen::Vector3f &force = controller_.getComputedForce(); diff --git a/trackers/kr_trackers/src/null_tracker.cpp b/trackers/kr_trackers/src/null_tracker.cpp index 95bd8592..a2ca792a 100644 --- a/trackers/kr_trackers/src/null_tracker.cpp +++ b/trackers/kr_trackers/src/null_tracker.cpp @@ -33,7 +33,8 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr NullTracker::update(const nav_ { // Return a null message (will not publish the position command) - return std::make_shared(); + // return std::make_shared(); + return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); } uint8_t NullTracker::status() From bfa0f57b5620a69823bc11695309530623efbfb1 Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Thu, 27 Mar 2025 16:04:58 -0400 Subject: [PATCH 36/64] fixed take off, mass was off, onto goto --- .../src/so3_control_component.cpp | 4 +- kr_mav_manager/src/manager.cpp | 2 +- log3880012195.csv | 526 ------------------ params3679191350.csv | 311 ----------- .../src/line_tracker_distance_server.cpp | 112 ---- .../src/line_tracker_min_jerk_server.cpp | 69 +-- 6 files changed, 39 insertions(+), 985 deletions(-) delete mode 100644 log3880012195.csv delete mode 100644 params3679191350.csv diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index ab6516b0..5105ca7c 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -363,14 +363,16 @@ SO3ControlComponent::SO3ControlComponent(const rclcpp::NodeOptions &options) controller_.resetIntegrals(); this->declare_parameter("quadrotor_name", std::string("quadrotor")); - this->declare_parameter("mass", 0.5f); + this->declare_parameter("mass", 0.03f); std::string quadrotor_name; this->get_parameter("quadrotor_name", quadrotor_name); frame_id_ = "/" + quadrotor_name; this->get_parameter("mass", mass_); + controller_.setMass(mass_); + RCLCPP_INFO_STREAM(this->get_logger(), "MASS FROM SO3: " << mass_); controller_.setGravity(g_); this->declare_parameter("use_external_yaw", true); diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index c60e4184..99834166 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -430,7 +430,7 @@ bool MAVManager::goTo(float x, float y, float z, float yaw, float v_des, float a options.result_callback = std::bind(&MAVManager::tracker_done_callback, this, _1); line_tracker_min_jerk_client_->async_send_goal(goal, options); - + std::this_thread::sleep_for(std::chrono::seconds(1)); return this->transition(line_tracker_min_jerk); } diff --git a/log3880012195.csv b/log3880012195.csv deleted file mode 100644 index 997decf7..00000000 --- a/log3880012195.csv +++ /dev/null @@ -1,526 +0,0 @@ -id,type,group,name -0,1,activeMarker,btSns -1,1,activeMarker,i2cOk -2,1,motion,motion -3,5,motion,deltaX -4,5,motion,deltaY -5,2,motion,shutter -6,1,motion,maxRaw -7,1,motion,minRaw -8,1,motion,Rawsum -9,1,motion,outlierCount -10,1,motion,squal -11,7,motion,std -12,7,ring,fadeTime -13,1,loco,mode -14,7,loco,spiWr -15,7,loco,spiRe -16,2,ranging,state -17,7,ranging,distance0 -18,7,ranging,distance1 -19,7,ranging,distance2 -20,7,ranging,distance3 -21,7,ranging,distance4 -22,7,ranging,distance5 -23,7,ranging,distance6 -24,7,ranging,distance7 -25,7,ranging,pressure0 -26,7,ranging,pressure1 -27,7,ranging,pressure2 -28,7,ranging,pressure3 -29,7,ranging,pressure4 -30,7,ranging,pressure5 -31,7,ranging,pressure6 -32,7,ranging,pressure7 -33,7,tdoa2,d7-0 -34,7,tdoa2,d0-1 -35,7,tdoa2,d1-2 -36,7,tdoa2,d2-3 -37,7,tdoa2,d3-4 -38,7,tdoa2,d4-5 -39,7,tdoa2,d5-6 -40,7,tdoa2,d6-7 -41,7,tdoa2,cc0 -42,7,tdoa2,cc1 -43,7,tdoa2,cc2 -44,7,tdoa2,cc3 -45,7,tdoa2,cc4 -46,7,tdoa2,cc5 -47,7,tdoa2,cc6 -48,7,tdoa2,cc7 -49,2,tdoa2,dist7-0 -50,2,tdoa2,dist0-1 -51,2,tdoa2,dist1-2 -52,2,tdoa2,dist2-3 -53,2,tdoa2,dist3-4 -54,2,tdoa2,dist4-5 -55,2,tdoa2,dist5-6 -56,2,tdoa2,dist6-7 -57,1,twr,rangingSuccessRate0 -58,1,twr,rangingPerSec0 -59,1,twr,rangingSuccessRate1 -60,1,twr,rangingPerSec1 -61,1,twr,rangingSuccessRate2 -62,1,twr,rangingPerSec2 -63,1,twr,rangingSuccessRate3 -64,1,twr,rangingPerSec3 -65,1,twr,rangingSuccessRate4 -66,1,twr,rangingPerSec4 -67,1,twr,rangingSuccessRate5 -68,1,twr,rangingPerSec5 -69,2,oa,front -70,2,oa,back -71,2,oa,up -72,2,oa,left -73,2,oa,right -74,7,usd,spiWrBps -75,7,usd,spiReBps -76,7,usd,fatWrBps -77,2,rpm,m1 -78,2,rpm,m2 -79,2,rpm,m3 -80,2,rpm,m4 -81,2,motor,m1 -82,2,motor,m2 -83,2,motor,m3 -84,2,motor,m4 -85,6,motor,m1req -86,6,motor,m2req -87,6,motor,m3req -88,6,motor,m4req -89,7,pm,vbat -90,2,pm,vbatMV -91,7,pm,extVbat -92,2,pm,extVbatMV -93,7,pm,extCurr -94,7,pm,chargeCurrent -95,4,pm,state -96,1,pm,batteryLevel -97,1,radio,rssi -98,1,radio,isConnected -99,2,radio,numRxBc -100,2,radio,numRxUc -101,5,gyro,xRaw -102,5,gyro,yRaw -103,5,gyro,zRaw -104,7,gyro,xVariance -105,7,gyro,yVariance -106,7,gyro,zVariance -107,7,gyro,x -108,7,gyro,y -109,7,gyro,z -110,3,colAv,latency -111,7,ext_pos,X -112,7,ext_pos,Y -113,7,ext_pos,Z -114,7,locSrv,x -115,7,locSrv,y -116,7,locSrv,z -117,7,locSrv,qx -118,7,locSrv,qy -119,7,locSrv,qz -120,7,locSrv,qw -121,2,locSrvZ,tick -122,2,crtp,rxRate -123,2,crtp,txRate -124,7,extrx,thrust -125,7,extrx,roll -126,7,extrx,pitch -127,7,extrx,rollRate -128,7,extrx,pitchRate -129,7,extrx,yawRate -130,7,extrx,zVel -131,1,extrx,AltHold -132,1,extrx,Arm -133,2,extrx_raw,ch0 -134,2,extrx_raw,ch1 -135,2,extrx_raw,ch2 -136,2,extrx_raw,ch3 -137,2,extrx_raw,ch4 -138,2,extrx_raw,ch5 -139,2,extrx_raw,ch6 -140,2,extrx_raw,ch7 -141,7,health,motorVarXM1 -142,7,health,motorVarYM1 -143,7,health,motorVarXM2 -144,7,health,motorVarYM2 -145,7,health,motorVarXM3 -146,7,health,motorVarYM3 -147,7,health,motorVarXM4 -148,7,health,motorVarYM4 -149,1,health,motorPass -150,7,health,batterySag -151,1,health,batteryPass -152,2,health,motorTestCount -153,3,memTst,errCntW -154,2,range,front -155,2,range,back -156,2,range,up -157,2,range,left -158,2,range,right -159,2,range,zrange -160,7,sensfusion6,qw -161,7,sensfusion6,qx -162,7,sensfusion6,qy -163,7,sensfusion6,qz -164,7,sensfusion6,gravityX -165,7,sensfusion6,gravityY -166,7,sensfusion6,gravityZ -167,7,sensfusion6,accZbase -168,1,sensfusion6,isInit -169,1,sensfusion6,isCalibrated -170,7,acc,x -171,7,acc,y -172,7,acc,z -173,7,baro,asl -174,7,baro,temp -175,7,baro,pressure -176,5,controller,ctr_yaw -177,7,controller,cmd_thrust -178,7,controller,cmd_roll -179,7,controller,cmd_pitch -180,7,controller,cmd_yaw -181,7,controller,r_roll -182,7,controller,r_pitch -183,7,controller,r_yaw -184,7,controller,accelz -185,7,controller,actuatorThrust -186,7,controller,roll -187,7,controller,pitch -188,7,controller,yaw -189,7,controller,rollRate -190,7,controller,pitchRate -191,7,controller,yawRate -192,7,ctrltarget,x -193,7,ctrltarget,y -194,7,ctrltarget,z -195,7,ctrltarget,vx -196,7,ctrltarget,vy -197,7,ctrltarget,vz -198,7,ctrltarget,ax -199,7,ctrltarget,ay -200,7,ctrltarget,az -201,7,ctrltarget,roll -202,7,ctrltarget,pitch -203,7,ctrltarget,yaw -204,5,ctrltargetZ,x -205,5,ctrltargetZ,y -206,5,ctrltargetZ,z -207,5,ctrltargetZ,vx -208,5,ctrltargetZ,vy -209,5,ctrltargetZ,vz -210,5,ctrltargetZ,ax -211,5,ctrltargetZ,ay -212,5,ctrltargetZ,az -213,7,mag,x -214,7,mag,y -215,7,mag,z -216,7,stabilizer,roll -217,7,stabilizer,pitch -218,7,stabilizer,yaw -219,7,stabilizer,thrust -220,7,stabilizer,rtStab -221,3,stabilizer,intToOut -222,7,stateEstimate,x -223,7,stateEstimate,y -224,7,stateEstimate,z -225,7,stateEstimate,vx -226,7,stateEstimate,vy -227,7,stateEstimate,vz -228,7,stateEstimate,ax -229,7,stateEstimate,ay -230,7,stateEstimate,az -231,7,stateEstimate,roll -232,7,stateEstimate,pitch -233,7,stateEstimate,yaw -234,7,stateEstimate,qx -235,7,stateEstimate,qy -236,7,stateEstimate,qz -237,7,stateEstimate,qw -238,5,stateEstimateZ,x -239,5,stateEstimateZ,y -240,5,stateEstimateZ,z -241,5,stateEstimateZ,vx -242,5,stateEstimateZ,vy -243,5,stateEstimateZ,vz -244,5,stateEstimateZ,ax -245,5,stateEstimateZ,ay -246,5,stateEstimateZ,az -247,3,stateEstimateZ,quat -248,5,stateEstimateZ,rateRoll -249,5,stateEstimateZ,ratePitch -250,5,stateEstimateZ,rateYaw -251,2,supervisor,info -252,1,sys,canfly -253,1,sys,isFlying -254,1,sys,isTumbled -255,4,sys,testLogParam -256,7,tdoaEngine,stRx -257,7,tdoaEngine,stEst -258,7,tdoaEngine,stTime -259,7,tdoaEngine,stFound -260,7,tdoaEngine,stCc -261,7,tdoaEngine,stHit -262,7,tdoaEngine,stMiss -263,7,tdoaEngine,cc -264,2,tdoaEngine,tof -265,7,tdoaEngine,tdoa -266,7,kalman_pred,predNX -267,7,kalman_pred,predNY -268,7,kalman_pred,measNX -269,7,kalman_pred,measNY -270,1,lighthouse,validAngles -271,7,lighthouse,rawAngle0x -272,7,lighthouse,rawAngle0y -273,7,lighthouse,rawAngle1x -274,7,lighthouse,rawAngle1y -275,7,lighthouse,angle0x -276,7,lighthouse,angle0y -277,7,lighthouse,angle1x -278,7,lighthouse,angle1y -279,7,lighthouse,angle0x_1 -280,7,lighthouse,angle0y_1 -281,7,lighthouse,angle1x_1 -282,7,lighthouse,angle1y_1 -283,7,lighthouse,angle0x_2 -284,7,lighthouse,angle0y_2 -285,7,lighthouse,angle1x_2 -286,7,lighthouse,angle1y_2 -287,7,lighthouse,angle0x_3 -288,7,lighthouse,angle0y_3 -289,7,lighthouse,angle1x_3 -290,7,lighthouse,angle1y_3 -291,7,lighthouse,rawAngle0xlh2 -292,7,lighthouse,rawAngle0ylh2 -293,7,lighthouse,rawAngle1xlh2 -294,7,lighthouse,rawAngle1ylh2 -295,7,lighthouse,angle0x_0lh2 -296,7,lighthouse,angle0y_0lh2 -297,7,lighthouse,angle1x_0lh2 -298,7,lighthouse,angle1y_0lh2 -299,2,lighthouse,width0 -300,2,lighthouse,width1 -301,2,lighthouse,width2 -302,2,lighthouse,width3 -303,1,lighthouse,comSync -304,2,lighthouse,bsAvailable -305,2,lighthouse,bsReceive -306,2,lighthouse,bsActive -307,2,lighthouse,bsCalUd -308,2,lighthouse,bsCalCon -309,1,lighthouse,status -310,7,lighthouse,posRt -311,7,lighthouse,estBs0Rt -312,7,lighthouse,estBs1Rt -313,7,lighthouse,x -314,7,lighthouse,y -315,7,lighthouse,z -316,7,lighthouse,delta -317,2,lighthouse,bsGeoVal -318,2,lighthouse,bsCalVal -319,7,lighthouse,disProb -320,7,pid_attitude,roll_outP -321,7,pid_attitude,roll_outI -322,7,pid_attitude,roll_outD -323,7,pid_attitude,roll_outFF -324,7,pid_attitude,pitch_outP -325,7,pid_attitude,pitch_outI -326,7,pid_attitude,pitch_outD -327,7,pid_attitude,pitch_outFF -328,7,pid_attitude,yaw_outP -329,7,pid_attitude,yaw_outI -330,7,pid_attitude,yaw_outD -331,7,pid_attitude,yaw_outFF -332,7,pid_rate,roll_outP -333,7,pid_rate,roll_outI -334,7,pid_rate,roll_outD -335,7,pid_rate,roll_outFF -336,7,pid_rate,pitch_outP -337,7,pid_rate,pitch_outI -338,7,pid_rate,pitch_outD -339,7,pid_rate,pitch_outFF -340,7,pid_rate,yaw_outP -341,7,pid_rate,yaw_outI -342,7,pid_rate,yaw_outD -343,7,pid_rate,yaw_outFF -344,7,ctrlINDI,cmd_thrust -345,7,ctrlINDI,cmd_roll -346,7,ctrlINDI,cmd_pitch -347,7,ctrlINDI,cmd_yaw -348,7,ctrlINDI,r_roll -349,7,ctrlINDI,r_pitch -350,7,ctrlINDI,r_yaw -351,7,ctrlINDI,u_act_dyn_p -352,7,ctrlINDI,u_act_dyn_q -353,7,ctrlINDI,u_act_dyn_r -354,7,ctrlINDI,du_p -355,7,ctrlINDI,du_q -356,7,ctrlINDI,du_r -357,7,ctrlINDI,ang_accel_ref_p -358,7,ctrlINDI,ang_accel_ref_q -359,7,ctrlINDI,ang_accel_ref_r -360,7,ctrlINDI,rate_d[0] -361,7,ctrlINDI,rate_d[1] -362,7,ctrlINDI,rate_d[2] -363,7,ctrlINDI,uf_p -364,7,ctrlINDI,uf_q -365,7,ctrlINDI,uf_r -366,7,ctrlINDI,Omega_f_p -367,7,ctrlINDI,Omega_f_q -368,7,ctrlINDI,Omega_f_r -369,7,ctrlINDI,n_p -370,7,ctrlINDI,n_q -371,7,ctrlINDI,n_r -372,7,ctrlMel,cmd_thrust -373,7,ctrlMel,cmd_roll -374,7,ctrlMel,cmd_pitch -375,7,ctrlMel,cmd_yaw -376,7,ctrlMel,r_roll -377,7,ctrlMel,r_pitch -378,7,ctrlMel,r_yaw -379,7,ctrlMel,accelz -380,7,ctrlMel,zdx -381,7,ctrlMel,zdy -382,7,ctrlMel,zdz -383,7,ctrlMel,i_err_x -384,7,ctrlMel,i_err_y -385,7,ctrlMel,i_err_z -386,7,posCtrlIndi,posRef_x -387,7,posCtrlIndi,posRef_y -388,7,posCtrlIndi,posRef_z -389,7,posCtrlIndi,velS_x -390,7,posCtrlIndi,velS_y -391,7,posCtrlIndi,velS_z -392,7,posCtrlIndi,velRef_x -393,7,posCtrlIndi,velRef_y -394,7,posCtrlIndi,velRef_z -395,7,posCtrlIndi,angS_roll -396,7,posCtrlIndi,angS_pitch -397,7,posCtrlIndi,angS_yaw -398,7,posCtrlIndi,angF_roll -399,7,posCtrlIndi,angF_pitch -400,7,posCtrlIndi,angF_yaw -401,7,posCtrlIndi,accRef_x -402,7,posCtrlIndi,accRef_y -403,7,posCtrlIndi,accRef_z -404,7,posCtrlIndi,accS_x -405,7,posCtrlIndi,accS_y -406,7,posCtrlIndi,accS_z -407,7,posCtrlIndi,accF_x -408,7,posCtrlIndi,accF_y -409,7,posCtrlIndi,accF_z -410,7,posCtrlIndi,accFT_x -411,7,posCtrlIndi,accFT_y -412,7,posCtrlIndi,accFT_z -413,7,posCtrlIndi,accErr_x -414,7,posCtrlIndi,accErr_y -415,7,posCtrlIndi,accErr_z -416,7,posCtrlIndi,phi_tilde -417,7,posCtrlIndi,theta_tilde -418,7,posCtrlIndi,T_tilde -419,7,posCtrlIndi,T_inner -420,7,posCtrlIndi,T_inner_f -421,7,posCtrlIndi,T_incremented -422,7,posCtrlIndi,cmd_phi -423,7,posCtrlIndi,cmd_theta -424,7,posCtl,targetVX -425,7,posCtl,targetVY -426,7,posCtl,targetVZ -427,7,posCtl,targetX -428,7,posCtl,targetY -429,7,posCtl,targetZ -430,7,posCtl,bodyVX -431,7,posCtl,bodyVY -432,7,posCtl,bodyX -433,7,posCtl,bodyY -434,7,posCtl,Xp -435,7,posCtl,Xi -436,7,posCtl,Xd -437,7,posCtl,Xff -438,7,posCtl,Yp -439,7,posCtl,Yi -440,7,posCtl,Yd -441,7,posCtl,Yff -442,7,posCtl,Zp -443,7,posCtl,Zi -444,7,posCtl,Zd -445,7,posCtl,Zff -446,7,posCtl,VXp -447,7,posCtl,VXi -448,7,posCtl,VXd -449,7,posCtl,VXff -450,7,posCtl,VYp -451,7,posCtl,VYi -452,7,posCtl,VYd -453,7,posCtl,VYff -454,7,posCtl,VZp -455,7,posCtl,VZi -456,7,posCtl,VZd -457,7,posCtl,VZff -458,7,ctrlLee,KR_x -459,7,ctrlLee,KR_y -460,7,ctrlLee,KR_z -461,7,ctrlLee,Kw_x -462,7,ctrlLee,Kw_y -463,7,ctrlLee,Kw_z -464,7,ctrlLee,Kpos_Px -465,7,ctrlLee,Kpos_Py -466,7,ctrlLee,Kpos_Pz -467,7,ctrlLee,Kpos_Dx -468,7,ctrlLee,Kpos_Dy -469,7,ctrlLee,Kpos_Dz -470,7,ctrlLee,thrustSi -471,7,ctrlLee,torquex -472,7,ctrlLee,torquey -473,7,ctrlLee,torquez -474,7,ctrlLee,rpyx -475,7,ctrlLee,rpyy -476,7,ctrlLee,rpyz -477,7,ctrlLee,rpydx -478,7,ctrlLee,rpydy -479,7,ctrlLee,rpydz -480,7,ctrlLee,error_posx -481,7,ctrlLee,error_posy -482,7,ctrlLee,error_posz -483,7,ctrlLee,error_velx -484,7,ctrlLee,error_vely -485,7,ctrlLee,error_velz -486,7,ctrlLee,omegax -487,7,ctrlLee,omegay -488,7,ctrlLee,omegaz -489,7,ctrlLee,omegarx -490,7,ctrlLee,omegary -491,7,ctrlLee,omegarz -492,7,kalman,stateX -493,7,kalman,stateY -494,7,kalman,stateZ -495,7,kalman,statePX -496,7,kalman,statePY -497,7,kalman,statePZ -498,7,kalman,stateD0 -499,7,kalman,stateD1 -500,7,kalman,stateD2 -501,7,kalman,varX -502,7,kalman,varY -503,7,kalman,varZ -504,7,kalman,varPX -505,7,kalman,varPY -506,7,kalman,varPZ -507,7,kalman,varD0 -508,7,kalman,varD1 -509,7,kalman,varD2 -510,7,kalman,q0 -511,7,kalman,q1 -512,7,kalman,q2 -513,7,kalman,q3 -514,7,kalman,rtUpdate -515,7,kalman,rtPred -516,7,kalman,rtFinal -517,6,outlierf,lhWin -518,7,estimator,rtApnd -519,7,estimator,rtRej -520,7,posEstAlt,estimatedZ -521,7,posEstAlt,estVZ -522,7,posEstAlt,velocityZ -523,1,DTR_P2P,rx_state -524,1,DTR_P2P,tx_state diff --git a/params3679191350.csv b/params3679191350.csv deleted file mode 100644 index 9e807627..00000000 --- a/params3679191350.csv +++ /dev/null @@ -1,311 +0,0 @@ -id,type,readonly,group,name -0,8,0,radiotest,channel -1,0,0,radiotest,power -2,8,0,radiotest,contwave -3,8,0,activeMarker,front -4,8,0,activeMarker,back -5,8,0,activeMarker,left -6,8,0,activeMarker,right -7,8,0,activeMarker,mode -8,8,0,activeMarker,poll -9,8,1,deck,bcActiveMarker -10,8,1,deck,bcAI -11,8,1,deck,bcBuzzer -12,8,1,deck,bcFlow -13,8,1,deck,bcFlow2 -14,8,1,deck,bcLedRing -15,8,1,deck,bcLighthouse4 -16,8,1,deck,bcDWM1000 -17,8,1,deck,bcLoco -18,8,1,deck,bcMultiranger -19,8,1,deck,bcOA -20,8,1,deck,bcUSD -21,8,1,deck,bcZRanger -22,8,1,deck,bcZRanger2 -23,8,0,motion,disable -24,8,0,motion,adaptive -25,6,0,motion,flowStdFixed -26,8,0,ring,effect -27,10,1,ring,neffect -28,8,0,ring,solidRed -29,8,0,ring,solidGreen -30,8,0,ring,solidBlue -31,8,0,ring,headlightEnable -32,6,0,ring,emptyCharge -33,6,0,ring,fullCharge -34,10,0,ring,fadeColor -35,6,0,ring,fadeTime -36,8,0,system,highlight -37,8,0,system,storageStats -38,8,0,system,storageReformat -39,8,0,system,taskDump -40,0,1,system,selftestPassed -41,8,0,system,assertInfo -42,8,0,system,testLogParam -43,8,0,system,doAssert -44,8,0,loco,mode -45,6,0,tdoa2,stddev -46,6,0,tdoa3,stddev -47,9,0,multiranger,filterMask -48,8,1,usd,canLog -49,8,0,usd,logging -50,8,0,led,bitmask -51,8,0,motorPowerSet,enable -52,9,0,motorPowerSet,m1 -53,9,0,motorPowerSet,m2 -54,9,0,motorPowerSet,m3 -55,9,0,motorPowerSet,m4 -56,6,0,pm,lowVoltage -57,6,0,pm,criticalLowVoltage -58,8,1,imu_sensors,BMP3XX -59,6,0,imu_sensors,imuPhi -60,6,0,imu_sensors,imuTheta -61,6,0,imu_sensors,imuPsi -62,8,1,imu_sensors,AK8963 -63,8,1,imu_sensors,LPS25H -64,8,1,imu_tests,MPU6500 -65,8,1,imu_tests,AK8963 -66,8,1,imu_tests,LPS25H -67,6,0,imu_tests,imuPhi -68,6,0,imu_tests,imuTheta -69,6,0,imu_tests,imuPsi -70,8,0,syslink,probe -71,8,0,usec,reset -72,8,0,colAv,enable -73,6,0,colAv,ellipsoidX -74,6,0,colAv,ellipsoidY -75,6,0,colAv,ellipsoidZ -76,6,0,colAv,bboxMinX -77,6,0,colAv,bboxMinY -78,6,0,colAv,bboxMinZ -79,6,0,colAv,bboxMaxX -80,6,0,colAv,bboxMaxY -81,6,0,colAv,bboxMaxZ -82,6,0,colAv,horizon -83,6,0,colAv,maxSpeed -84,6,0,colAv,sidestepThrsh -85,2,0,colAv,maxPeerLocAge -86,6,0,colAv,vorTol -87,2,0,colAv,vorIters -88,8,0,commander,enHighLevel -89,6,0,cppm,rateRoll -90,6,0,cppm,ratePitch -91,6,0,cppm,angPitch -92,6,0,cppm,angRoll -93,6,0,cppm,rateYaw -94,6,0,hlCommander,vtoff -95,6,0,hlCommander,vland -96,8,0,hlCommander,groupmask -97,8,0,flightmode,althold -98,8,0,flightmode,poshold -99,8,0,flightmode,posSet -100,8,0,flightmode,yawMode -101,8,0,flightmode,stabModeRoll -102,8,0,flightmode,stabModePitch -103,8,0,flightmode,stabModeYaw -104,8,0,locSrv,enRangeStreamFP32 -105,8,0,locSrv,enLhAngleStream -106,6,0,locSrv,extPosStdDev -107,6,0,locSrv,extQuatStdDev -108,9,0,crtpsrv,echoDelay -109,8,0,health,startPropTest -110,8,0,health,startBatTest -111,9,0,health,propTestPWMRatio -112,9,0,health,batTestPWMRatio -113,6,0,kalman,maxPos -114,6,0,kalman,maxVel -115,8,0,kalman,resetEstimation -116,8,0,kalman,robustTdoa -117,8,0,kalman,robustTwr -118,6,0,kalman,pNAcc_xy -119,6,0,kalman,pNAcc_z -120,6,0,kalman,pNVel -121,6,0,kalman,pNPos -122,6,0,kalman,pNAtt -123,6,0,kalman,mNBaro -124,6,0,kalman,mNGyro_rollpitch -125,6,0,kalman,mNGyro_yaw -126,6,0,kalman,initialX -127,6,0,kalman,initialY -128,6,0,kalman,initialZ -129,6,0,kalman,initialYaw -130,8,0,memTst,resetW -131,10,0,powerDist,idleThrust -132,6,0,quadSysId,thrustToTorque -133,6,0,quadSysId,pwmToThrustA -134,6,0,quadSysId,pwmToThrustB -135,6,0,quadSysId,armLength -136,6,0,sensfusion6,kp -137,6,0,sensfusion6,ki -138,6,0,sensfusion6,baseZacc -139,8,0,sound,effect -140,10,1,sound,neffect -141,9,0,sound,freq -142,8,0,stabilizer,estimator -143,8,0,stabilizer,controller -144,8,0,stabilizer,stop -145,8,0,supervisor,infdmp -146,9,0,supervisor,prefltTimeout -147,9,0,supervisor,landedTimeout -148,8,0,supervisor,tmblChckEn -149,9,1,cpu,flash -150,10,1,cpu,id0 -151,10,1,cpu,id1 -152,10,1,cpu,id2 -153,8,0,tdoaEngine,logId -154,8,0,tdoaEngine,logOthrId -155,8,0,tdoaEngine,matchAlgo -156,8,0,lighthouse,method -157,8,0,lighthouse,bsCalibReset -158,8,0,lighthouse,systemType -159,9,1,lighthouse,bsAvailable -160,6,0,lighthouse,sweepStd -161,6,0,lighthouse,sweepStd2 -162,8,0,lighthouse,enLhRawStream -163,9,0,lighthouse,lh2maxRate -164,6,0,pid_attitude,roll_kp -165,6,0,pid_attitude,roll_ki -166,6,0,pid_attitude,roll_kd -167,6,0,pid_attitude,roll_kff -168,6,0,pid_attitude,pitch_kp -169,6,0,pid_attitude,pitch_ki -170,6,0,pid_attitude,pitch_kd -171,6,0,pid_attitude,pitch_kff -172,6,0,pid_attitude,yaw_kp -173,6,0,pid_attitude,yaw_ki -174,6,0,pid_attitude,yaw_kd -175,6,0,pid_attitude,yaw_kff -176,6,0,pid_attitude,yawMaxDelta -177,0,0,pid_attitude,attFiltEn -178,6,0,pid_attitude,attFiltCut -179,6,0,pid_rate,roll_kp -180,6,0,pid_rate,roll_ki -181,6,0,pid_rate,roll_kd -182,6,0,pid_rate,roll_kff -183,6,0,pid_rate,pitch_kp -184,6,0,pid_rate,pitch_ki -185,6,0,pid_rate,pitch_kd -186,6,0,pid_rate,pitch_kff -187,6,0,pid_rate,yaw_kp -188,6,0,pid_rate,yaw_ki -189,6,0,pid_rate,yaw_kd -190,6,0,pid_rate,yaw_kff -191,0,0,pid_rate,rateFiltEn -192,6,0,pid_rate,omxFiltCut -193,6,0,pid_rate,omyFiltCut -194,6,0,pid_rate,omzFiltCut -195,6,0,ctrlINDI,thrust_threshold -196,6,0,ctrlINDI,bound_ctrl_input -197,6,0,ctrlINDI,g1_p -198,6,0,ctrlINDI,g1_q -199,6,0,ctrlINDI,g1_r -200,6,0,ctrlINDI,g2 -201,6,0,ctrlINDI,ref_err_p -202,6,0,ctrlINDI,ref_err_q -203,6,0,ctrlINDI,ref_err_r -204,6,0,ctrlINDI,ref_rate_p -205,6,0,ctrlINDI,ref_rate_q -206,6,0,ctrlINDI,ref_rate_r -207,6,0,ctrlINDI,act_dyn_p -208,6,0,ctrlINDI,act_dyn_q -209,6,0,ctrlINDI,act_dyn_r -210,6,0,ctrlINDI,filt_cutoff -211,6,0,ctrlINDI,filt_cutoff_r -212,8,0,ctrlINDI,outerLoopActive -213,6,0,ctrlMel,kp_xy -214,6,0,ctrlMel,kd_xy -215,6,0,ctrlMel,ki_xy -216,6,0,ctrlMel,i_range_xy -217,6,0,ctrlMel,kp_z -218,6,0,ctrlMel,kd_z -219,6,0,ctrlMel,ki_z -220,6,0,ctrlMel,i_range_z -221,6,0,ctrlMel,mass -222,6,0,ctrlMel,massThrust -223,6,0,ctrlMel,kR_xy -224,6,0,ctrlMel,kR_z -225,6,0,ctrlMel,kw_xy -226,6,0,ctrlMel,kw_z -227,6,0,ctrlMel,ki_m_xy -228,6,0,ctrlMel,ki_m_z -229,6,0,ctrlMel,kd_omega_rp -230,6,0,ctrlMel,i_range_m_xy -231,6,0,ctrlMel,i_range_m_z -232,6,0,ctrlAtt,tau_xy -233,6,0,ctrlAtt,zeta_xy -234,6,0,ctrlAtt,tau_z -235,6,0,ctrlAtt,zeta_z -236,6,0,ctrlAtt,tau_rp -237,6,0,ctrlAtt,mixing_factor -238,6,0,ctrlAtt,coll_fairness -239,6,0,posCtrlIndi,K_xi_x -240,6,0,posCtrlIndi,K_xi_y -241,6,0,posCtrlIndi,K_xi_z -242,6,0,posCtrlIndi,K_dxi_x -243,6,0,posCtrlIndi,K_dxi_y -244,6,0,posCtrlIndi,K_dxi_z -245,6,0,posCtrlIndi,pq_clamping -246,6,0,posCtlPid,xKp -247,6,0,posCtlPid,xKi -248,6,0,posCtlPid,xKd -249,6,0,posCtlPid,xKff -250,6,0,posCtlPid,yKp -251,6,0,posCtlPid,yKi -252,6,0,posCtlPid,yKd -253,6,0,posCtlPid,yKff -254,6,0,posCtlPid,zKp -255,6,0,posCtlPid,zKi -256,6,0,posCtlPid,zKd -257,6,0,posCtlPid,zKff -258,9,0,posCtlPid,thrustBase -259,9,0,posCtlPid,thrustMin -260,6,0,posCtlPid,rLimit -261,6,0,posCtlPid,pLimit -262,6,0,posCtlPid,xVelMax -263,6,0,posCtlPid,yVelMax -264,6,0,posCtlPid,zVelMax -265,6,0,velCtlPid,vxKp -266,6,0,velCtlPid,vxKi -267,6,0,velCtlPid,vxKd -268,6,0,velCtlPid,vxKFF -269,6,0,velCtlPid,vyKp -270,6,0,velCtlPid,vyKi -271,6,0,velCtlPid,vyKd -272,6,0,velCtlPid,vyKFF -273,6,0,velCtlPid,vzKp -274,6,0,velCtlPid,vzKi -275,6,0,velCtlPid,vzKd -276,6,0,velCtlPid,vzKFF -277,6,0,ctrlLee,KR_x -278,6,0,ctrlLee,KR_y -279,6,0,ctrlLee,KR_z -280,6,0,ctrlLee,KI_x -281,6,0,ctrlLee,KI_y -282,6,0,ctrlLee,KI_z -283,6,0,ctrlLee,Kw_x -284,6,0,ctrlLee,Kw_y -285,6,0,ctrlLee,Kw_z -286,6,0,ctrlLee,J_x -287,6,0,ctrlLee,J_y -288,6,0,ctrlLee,J_z -289,6,0,ctrlLee,Kpos_Px -290,6,0,ctrlLee,Kpos_Py -291,6,0,ctrlLee,Kpos_Pz -292,6,0,ctrlLee,Kpos_P_limit -293,6,0,ctrlLee,Kpos_Dx -294,6,0,ctrlLee,Kpos_Dy -295,6,0,ctrlLee,Kpos_Dz -296,6,0,ctrlLee,Kpos_D_limit -297,6,0,ctrlLee,Kpos_Ix -298,6,0,ctrlLee,Kpos_Iy -299,6,0,ctrlLee,Kpos_Iz -300,6,0,ctrlLee,Kpos_I_limit -301,6,0,ctrlLee,mass -302,6,0,posEstAlt,estAlphaAsl -303,6,0,posEstAlt,estAlphaZr -304,6,0,posEstAlt,velFactor -305,6,0,posEstAlt,velZAlpha -306,6,0,posEstAlt,vAccDeadband -307,10,1,firmware,revision0 -308,9,1,firmware,revision1 -309,8,1,firmware,modified diff --git a/trackers/kr_trackers/src/line_tracker_distance_server.cpp b/trackers/kr_trackers/src/line_tracker_distance_server.cpp index d2c6150b..c4cadae0 100644 --- a/trackers/kr_trackers/src/line_tracker_distance_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_distance_server.cpp @@ -189,8 +189,6 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co result->duration = current_traj_duration_; result->length = current_traj_length_; result_ = result; - RCLCPP_INFO_STREAM(logger_, "goal_reached_" << goal_reached_); - RCLCPP_INFO(logger_, "UPDATE 1"); if (current_goal_handle_) { if(current_goal_handle_ && current_goal_handle_->is_canceling()) @@ -204,14 +202,12 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); } } - RCLCPP_INFO(logger_, "UPDATE 2"); auto cmd = std::make_shared(); cmd->header.stamp = clock_->now(); cmd->header.frame_id = msg->header.frame_id; cmd->yaw = start_yaw_; - RCLCPP_INFO(logger_, "UPDATE 3"); if(goal_reached_) { // RCLCPP_INFO_STREAM(logger_, "current_goal_handle: "<< current_goal_handle_); @@ -230,7 +226,6 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co return cmd; } - RCLCPP_INFO(logger_, "UPDATE 4"); const Eigen::Vector3f dir = (goal_ - start_).normalized(); const float total_dist = (goal_ - start_).norm(); const float d = (pos_ - start_).dot(dir); @@ -241,12 +236,9 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co Eigen::Vector3f x(pos_), v(Eigen::Vector3f::Zero()), a(Eigen::Vector3f::Zero()); - RCLCPP_INFO_STREAM(logger_, "pos_: " << pos_); - RCLCPP_INFO_STREAM(logger_, "goal_: " << goal_); if((pos_ - goal_).norm() <= epsilon_) // Reached goal { - RCLCPP_INFO(logger_, "UPDATE 5"); // Send success message and reset the length and duration variables result->duration = current_traj_duration_; result->length = current_traj_length_; @@ -269,7 +261,6 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co } else if(d > total_dist) // Overshoot { - RCLCPP_INFO(logger_, "UPDATE 6"); RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Overshoot"); a = -a_des_ * dir; v = Eigen::Vector3f::Zero(); @@ -277,7 +268,6 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co } else if(d >= (total_dist - ramp_dist) && d <= total_dist) // Decelerate { - RCLCPP_INFO(logger_, "UPDATE 7"); RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Decelerate"); a = -a_des_ * dir; v = std::sqrt(2 * a_des_ * (total_dist - d)) * dir; @@ -285,7 +275,6 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co } else if(d > ramp_dist && d < total_dist - ramp_dist) // Constant velocity { - RCLCPP_INFO(logger_, "UPDATE 8"); RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Constant velocity"); a = Eigen::Vector3f::Zero(); v = v_max * dir; @@ -293,7 +282,6 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co } else if(d >= 0 && d <= ramp_dist) // Accelerate { - RCLCPP_INFO(logger_, "UPDATE 9"); RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Accelerate"); a = a_des_ * dir; v = std::sqrt(2 * a_des_ * d) * dir; @@ -301,7 +289,6 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co } else if(d < 0) { - RCLCPP_INFO(logger_, "UPDATE 10"); RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Undershoot"); a = a_des_ * dir; v = Eigen::Vector3f::Zero(); @@ -314,107 +301,14 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co if(!goal_reached_) { - RCLCPP_INFO(logger_, "UPDATE 11"); auto feedback = std::make_shared(); feedback->distance_from_goal = (pos_ - goal_).norm(); current_goal_handle_->publish_feedback(feedback); } - RCLCPP_INFO(logger_, "UPDATE 12"); return cmd; } -// rclcpp_action::GoalResponse LineTrackerDistance::goal_callback(const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) -// { -// (void)uuid; -// (void)goal; -// // If another goal is already active, cancel that goal -// // and track this one instead -// std::lock_guard lock(mutex_); -// RCLCPP_INFO_STREAM(logger_, "current_goal_handle_: " << current_goal_handle_); -// -// if (current_goal_handle_ == 0) { -// RCLCPP_INFO_STREAM(logger_, "Initialization of line tracker detected"); -// goal_set_ = false; -// return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -// } -// -// if(current_goal_handle_ && current_goal_handle_->is_active()) -// { -// RCLCPP_INFO(logger_, "LineTrackerDistance goal (%2.2f, %2.2f, %2.2f) preempted.", goal_(0), goal_(1), goal_(2)); -// preempted_goal_id_ = current_goal_handle_->get_goal_id(); -// preempt_requested_ = true; -// -// goal_ = pos_; -// goal_set_ = false; -// goal_reached_ = false; -// } -// -// RCLCPP_INFO(logger_, "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); -// return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -// } -// -// rclcpp_action::CancelResponse LineTrackerDistance::cancel_callback(const std::shared_ptr goal_handle) -// { -// RCLCPP_INFO(logger_, "Received Cancel Request."); -// (void)goal_handle; -// -// goal_ = pos_; -// goal_set_ = false; -// goal_reached_ = false; -// -// return rclcpp_action::CancelResponse::ACCEPT; -// } -// -// rclcpp_action::ResultCode LineTrackerDistance::handle_accepted_callback(const std::shared_ptr goal_handle) -// { -// std::lock_guard lock(mutex_); -// -// if(current_goal_handle_ && preempt_requested_) -// { -// if(current_goal_handle_->get_goal_id() == preempted_goal_id_) -// { -// RCLCPP_INFO(logger_, "LineTrackerDistance going to goal (%2.2f, %2.2f, %2.2f) aborted.", goal_(0), goal_(1), goal_(2)); -// current_goal_handle_->abort(result_); -// preempt_requested_ = false; -// } -// } -// -// // Pointer to the goal received -// RCLCPP_INFO_STREAM(logger_, "New Goal: " << goal_handle); -// current_goal_handle_ = goal_handle; -// -// auto goal = goal_handle->get_goal(); -// goal_(0) = goal->x; -// goal_(1) = goal->y; -// goal_(2) = goal->z; -// -// if(goal->relative) -// goal_ += ICs_.pos(); -// -// if(goal->v_des > 0) -// v_des_ = goal->v_des; -// else -// v_des_ = default_v_des_; -// -// if(goal->a_des > 0) -// a_des_ = goal->a_des; -// else -// a_des_ = default_a_des_; -// -// start_ = pos_; -// start_yaw_ = yaw_; -// -// current_traj_length_ = 0.0; -// current_traj_duration_ = 0.0; -// -// goal_set_ = true; -// goal_reached_ = false; -// RCLCPP_INFO_STREAM(logger_, "Out of HAC: "); -// // auto result = std::make_shared(); -// // goal_handle->succeed(result); -// // return rclcpp_action::ResultCode::SUCCEEDED; -// } rclcpp_action::GoalResponse LineTrackerDistance::goal_callback(const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) { @@ -487,14 +381,8 @@ void LineTrackerDistance::handle_accepted_callback(const std::shared_ptr lock(mutex_); - // Pointer to the goal received - RCLCPP_INFO_STREAM(logger_, "New Goal: " << goal_handle); current_goal_handle_ = goal_handle; - RCLCPP_INFO_STREAM(logger_, "Out of HAC: "); - // auto result = std::make_shared(); - // goal_handle->succeed(result); - // return rclcpp_action::ResultCode::SUCCEEDED; } 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 5aed4ad2..ca95821a 100644 --- a/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp @@ -404,25 +404,6 @@ rclcpp_action::GoalResponse LineTrackerMinJerk::goal_callback(const rclcpp_actio goal_reached_ = false; } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse LineTrackerMinJerk::cancel_callback(const std::shared_ptr goal_handle) -{ - RCLCPP_INFO(logger_, "Received Cancel Request."); - (void)goal_handle; - - goal_ = ICs_.pos(); - goal_set_ = false; - goal_reached_ = true; - - return rclcpp_action::CancelResponse::ACCEPT; -} - -void LineTrackerMinJerk::handle_accepted_callback(const std::shared_ptr goal_handle) -{ - std::lock_guard lock(mutex_); - if(current_goal_handle_ && preempt_requested_) { if(current_goal_handle_->get_goal_id() == preempted_goal_id_) @@ -432,38 +413,58 @@ void LineTrackerMinJerk::handle_accepted_callback(const std::shared_ptrget_goal(); - goal_(0) = msg->x; - goal_(1) = msg->y; - goal_(2) = msg->z; - goal_yaw_ = msg->yaw; - goal_duration_ = msg->duration; - if(msg->t_start != rclcpp::Time()) + goal_(0) = goal->x; + goal_(1) = goal->y; + goal_(2) = goal->z; + goal_yaw_ = goal->yaw; + goal_duration_ = goal->duration; + if(goal->t_start != rclcpp::Time()) { - traj_start_ = msg->t_start; + traj_start_ = goal->t_start; traj_start_set_ = true; } else traj_start_set_ = false; - if(msg->relative) + if(goal->relative) { goal_ += ICs_.pos(); goal_yaw_ += ICs_.yaw(); RCLCPP_INFO(logger_, "line_tracker_min_jerk using relative command"); } - v_des_ = (msg->v_des > 0.0) ? msg->v_des : default_v_des_; - a_des_ = (msg->a_des > 0.0) ? msg->a_des : default_a_des_; + v_des_ = (goal->v_des > 0.0) ? goal->v_des : default_v_des_; + a_des_ = (goal->a_des > 0.0) ? goal->a_des : default_a_des_; RCLCPP_DEBUG(logger_, "line_tracker_min_jerk using v_des = %2.2f m/s and a_des = %2.2f m/s^2", v_des_, a_des_); goal_set_ = true; goal_reached_ = false; + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse LineTrackerMinJerk::cancel_callback(const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(logger_, "Received Cancel Request."); + (void)goal_handle; + + goal_ = ICs_.pos(); + goal_set_ = false; + goal_reached_ = true; + + return rclcpp_action::CancelResponse::ACCEPT; +} + +void LineTrackerMinJerk::handle_accepted_callback(const std::shared_ptr goal_handle) +{ + std::lock_guard lock(mutex_); + + + // Pointer to the goal received + current_goal_handle_ = goal_handle; + + // auto msg = goal_handle->get_goal(); } void LineTrackerMinJerk::gen_trajectory(const Eigen::Vector3f &xi, const Eigen::Vector3f &xf, const Eigen::Vector3f &vi, From c9b966ca3369e9a4a382eaec036e1438e2ad755b Mon Sep 17 00:00:00 2001 From: Kashish Garg Date: Fri, 28 Mar 2025 10:53:02 -0400 Subject: [PATCH 37/64] working goto and land --- kr_mav_manager/src/manager.cpp | 49 ++----------------- .../src/line_tracker_distance_server.cpp | 1 + .../src/line_tracker_min_jerk_server.cpp | 31 ++++++++++-- .../src/trackers_manager.cpp | 2 + 4 files changed, 32 insertions(+), 51 deletions(-) diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 99834166..99bbc4a4 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -282,20 +282,9 @@ bool MAVManager::takeoff() goal.z = takeoff_height_; goal.relative = true; - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - // Goal response callback - send_goal_options.goal_response_callback = - [this](const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle) { - if (!goal_handle) { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } else { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - return true; - } - }; - line_tracker_distance_client_->async_send_goal(goal, send_goal_options); + 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)); if(this->transition(line_tracker_distance)) { @@ -306,38 +295,6 @@ bool MAVManager::takeoff() return false; return true; - // auto future = std::async(std::launch::async, &MAVManager::send_takeoff_request, this, goal); - // if(future.get()) - // { - // RCLCPP_INFO(this->get_logger(), "got trueeeeeee"); - // if(this->transition(line_tracker_distance)) - // { - // status_ = FLYING; - // return true; - // } - // else - // return false; - // } -} - -bool MAVManager::send_takeoff_request(std::shared_ptr goal) -{ - - // // Create goal options with lambda callbacks - RCLCPP_INFO(this->get_logger(), "IN SEND TAKEOFF REQ"); - auto future = line_tracker_distance_client_->async_send_goal(*goal); - try - { - RCLCPP_INFO(this->get_logger(), "IN TRY"); - auto response = future.get(); - return true; - } - catch(const std::exception &e) - { - RCLCPP_INFO(this->get_logger(), "IN CATCH"); - RCLCPP_WARN(this->get_logger(), "Transition callback failed"); - } - return false; } bool MAVManager::set_mass(float m) diff --git a/trackers/kr_trackers/src/line_tracker_distance_server.cpp b/trackers/kr_trackers/src/line_tracker_distance_server.cpp index c4cadae0..8f5ae5ae 100644 --- a/trackers/kr_trackers/src/line_tracker_distance_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_distance_server.cpp @@ -383,6 +383,7 @@ void LineTrackerDistance::handle_accepted_callback(const std::shared_ptr lock_(mutex_); if(!current_goal_handle_ || !current_goal_handle_->is_active()) @@ -132,9 +134,11 @@ bool LineTrackerMinJerk::Activate(const kr_mav_msgs::msg::PositionCommand::Const } active_ = true; + RCLCPP_INFO(logger_, "LTMJ ACTIVATE 3"); current_traj_length_ = 0.0; } } + RCLCPP_INFO(logger_, "LTMJ ACTIVATE 4"); return active_; } @@ -191,7 +195,6 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerMinJerk::update(con if(current_goal_handle_ && current_goal_handle_->is_canceling()) { RCLCPP_INFO(logger_, "LineTrackerDistance going to goal (%2.2f, %2.2f, %2.2f) canceled.", goal_(0), goal_(1), goal_(2)); - current_goal_handle_->canceled(result); goal_ = current_pos_; goal_set_ = false; @@ -209,11 +212,14 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerMinJerk::update(con traj_start_ = t_now; bool duration_set = false; + RCLCPP_INFO(logger_, "setting traj_duration_ here first"); traj_duration_ = 0.5f; + RCLCPP_INFO_STREAM(logger_, "traj_duration: " << traj_duration_); // TODO: This should probably be after traj_duration_ is determined if(goal_duration_.seconds() > traj_duration_) { + RCLCPP_INFO(logger_, "setting traj_duration_ here second"); traj_duration_ = goal_duration_.seconds(); duration_set = true; } @@ -238,7 +244,9 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerMinJerk::update(con + (total_dist - ramping_distance) / v_des_ // Constant velocity + v_des_ / a_des_; // Ramp down + RCLCPP_INFO(logger_, "setting traj_duration_ here third"); traj_duration_ = std::max(traj_duration_, t); + RCLCPP_INFO_STREAM(logger_, "traj_duration: " << traj_duration_); } else { @@ -260,6 +268,8 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerMinJerk::update(con t_dir = -vo / a_des_ + std::sqrt(2.0f) * std::sqrt(vo * vo + 2.0f * a_des_ * total_dist) / a_des_; } + + RCLCPP_INFO(logger_, "setting traj_duration_ here fourth"); traj_duration_ = std::max(traj_duration_, t_dir); // The velocity component orthogonal to dir @@ -267,6 +277,7 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerMinJerk::update(con float t_non_dir = v_ortho / a_des_ // Ramp to zero velocity + std::sqrt(2.0f) * v_ortho / a_des_; // Get back to the dir line + RCLCPP_INFO(logger_, "setting traj_duration_ here 1"); traj_duration_ = std::max(traj_duration_, t_non_dir); } } @@ -287,10 +298,19 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerMinJerk::update(con // Consider yaw in the trajectory duration if(!duration_set) // Only if duration is not set from goal { - if(yaw_dist > yaw_v_des_ * yaw_v_des_ / yaw_a_des_) + if(yaw_dist > yaw_v_des_ * yaw_v_des_ / yaw_a_des_) { + RCLCPP_INFO(logger_, "setting traj_duration_ here 2"); traj_duration_ = std::max(traj_duration_, yaw_dist / yaw_v_des_ + yaw_v_des_ / yaw_a_des_); - else - traj_duration_ = std::max(traj_duration_, 2 * std::sqrt(yaw_dist / yaw_a_des_)); + } else { + // HERE IS THE PROBLEM + RCLCPP_INFO(logger_, "setting traj_duration_ here 3"); + if (yaw_a_des_ != 0) { + traj_duration_ = std::max(traj_duration_, 2 * std::sqrt(yaw_dist / yaw_a_des_)); + } + RCLCPP_INFO_STREAM(logger_, "traj_duration: " << traj_duration_); + RCLCPP_INFO_STREAM(logger_, "yaw_dist: " << yaw_dist); + RCLCPP_INFO_STREAM(logger_, "yaw_a_des: " << yaw_a_des_); + } } // TODO: Should consider yaw_dot_. See hover() in MAVManager @@ -436,7 +456,7 @@ rclcpp_action::GoalResponse LineTrackerMinJerk::goal_callback(const rclcpp_actio v_des_ = (goal->v_des > 0.0) ? goal->v_des : default_v_des_; a_des_ = (goal->a_des > 0.0) ? goal->a_des : default_a_des_; - RCLCPP_DEBUG(logger_, "line_tracker_min_jerk using v_des = %2.2f m/s and a_des = %2.2f m/s^2", v_des_, a_des_); + RCLCPP_INFO(logger_, "line_tracker_min_jerk using v_des = %2.2f m/s and a_des = %2.2f m/s^2", v_des_, a_des_); goal_set_ = true; goal_reached_ = false; @@ -464,6 +484,7 @@ void LineTrackerMinJerk::handle_accepted_callback(const std::shared_ptrget_goal(); } diff --git a/trackers/kr_trackers_manager/src/trackers_manager.cpp b/trackers/kr_trackers_manager/src/trackers_manager.cpp index 8b857461..58fce3a7 100644 --- a/trackers/kr_trackers_manager/src/trackers_manager.cpp +++ b/trackers/kr_trackers_manager/src/trackers_manager.cpp @@ -169,12 +169,14 @@ void TrackersManager::transition_callback(const kr_tracker_msgs::srv::Transition RCLCPP_INFO_STREAM(this->get_logger(), res->message); return; } + RCLCPP_INFO(this->get_logger(), "TRANSITION CALLBACK 2:"); if(active_tracker_ != NULL) { active_tracker_->Deactivate(); } + RCLCPP_INFO(this->get_logger(), "TRANSITION CALLBACK 3:"); active_tracker_ = it->second; res->success = true; res->message = std::string("Successfully activated tracker ") + req->tracker; From ca9c2d600428be97d4d2ec132e5a19145af39f9a Mon Sep 17 00:00:00 2001 From: Dexter Date: Tue, 1 Jul 2025 16:36:23 +0000 Subject: [PATCH 38/64] Initial ros2 port of sbus_interface --- interfaces/kr_sbus_interface/CMakeLists.txt | 63 ++ .../kr_sbus_interface/cmake/FindSnav.cmake | 21 + .../kr_sbus_interface/config/gains.yaml | 15 + .../config/mav_manager_params.yaml | 6 + .../kr_sbus_interface/config/neurofly.yaml | 32 + .../config/tracker_params.yaml | 25 + .../kr_sbus_interface/config/trackers.yaml | 11 + .../kr_sbus_interface/channel_mapping.h | 18 + .../include/kr_sbus_interface/sbus_bridge.h | 149 ++++ .../include/kr_sbus_interface/sbus_msg.h | 61 ++ .../kr_sbus_interface/sbus_serial_port.h | 49 ++ .../kr_sbus_interface/thrust_mapping.h | 39 ++ .../launch/sbus_interface.py | 57 ++ .../kr_sbus_interface/msg/SbusRosMessage.msg | 14 + .../kr_sbus_interface/nodelet_plugin.xml | 7 + interfaces/kr_sbus_interface/package.xml | 23 + .../scripts/mavlink_telemetry.py | 35 + .../kr_sbus_interface/src/sbus_bridge.cpp | 662 ++++++++++++++++++ interfaces/kr_sbus_interface/src/sbus_msg.cpp | 112 +++ .../src/sbus_serial_port.cpp | 398 +++++++++++ .../src/so3cmd_to_sbus_component.cpp | 156 +++++ .../kr_sbus_interface/src/thrust_mapping.cpp | 85 +++ 22 files changed, 2038 insertions(+) create mode 100644 interfaces/kr_sbus_interface/CMakeLists.txt create mode 100644 interfaces/kr_sbus_interface/cmake/FindSnav.cmake create mode 100644 interfaces/kr_sbus_interface/config/gains.yaml create mode 100644 interfaces/kr_sbus_interface/config/mav_manager_params.yaml create mode 100644 interfaces/kr_sbus_interface/config/neurofly.yaml create mode 100644 interfaces/kr_sbus_interface/config/tracker_params.yaml create mode 100644 interfaces/kr_sbus_interface/config/trackers.yaml create mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h create mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h create mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h create mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h create mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/thrust_mapping.h create mode 100644 interfaces/kr_sbus_interface/launch/sbus_interface.py create mode 100644 interfaces/kr_sbus_interface/msg/SbusRosMessage.msg create mode 100644 interfaces/kr_sbus_interface/nodelet_plugin.xml create mode 100644 interfaces/kr_sbus_interface/package.xml create mode 100644 interfaces/kr_sbus_interface/scripts/mavlink_telemetry.py create mode 100644 interfaces/kr_sbus_interface/src/sbus_bridge.cpp create mode 100644 interfaces/kr_sbus_interface/src/sbus_msg.cpp create mode 100644 interfaces/kr_sbus_interface/src/sbus_serial_port.cpp create mode 100644 interfaces/kr_sbus_interface/src/so3cmd_to_sbus_component.cpp create mode 100644 interfaces/kr_sbus_interface/src/thrust_mapping.cpp diff --git a/interfaces/kr_sbus_interface/CMakeLists.txt b/interfaces/kr_sbus_interface/CMakeLists.txt new file mode 100644 index 00000000..d3096411 --- /dev/null +++ b/interfaces/kr_sbus_interface/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.10) +project(kr_sbus_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include_directories( + include + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(kr_mav_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) + +add_library(${PROJECT_NAME} SHARED src/so3cmd_to_sbus_component.cpp + src/sbus_bridge.cpp + src/sbus_msg.cpp + src/sbus_serial_port.cpp) +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_components + kr_mav_msgs + nav_msgs + sensor_msgs + geometry_msgs + tf2) +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) +rclcpp_components_register_nodes(${PROJECT_NAME} "SO3CmdToSBUS") + +# Install headers +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/interfaces/kr_sbus_interface/cmake/FindSnav.cmake b/interfaces/kr_sbus_interface/cmake/FindSnav.cmake new file mode 100644 index 00000000..07f3cd69 --- /dev/null +++ b/interfaces/kr_sbus_interface/cmake/FindSnav.cmake @@ -0,0 +1,21 @@ +FIND_PATH(Snav_INCLUDE_DIR + NAMES + snav/snapdragon_navigator.h + PATHS + /usr/include + NO_DEFAULT_PATH +) + +FIND_LIBRARY(Snav_LIBRARY + NAMES + snav_arm + PATHS + /usr/lib + NO_DEFAULT_PATH +) + +IF(Snav_INCLUDE_DIR AND Snav_LIBRARY) + SET(Snav_FOUND TRUE) +ELSE(Snav_INCLUDE_DIR AND Snav_LIBRARY) + SET(Snav_FOUND FALSE) +ENDIF(Snav_INCLUDE_DIR AND Snav_LIBRARY) diff --git a/interfaces/kr_sbus_interface/config/gains.yaml b/interfaces/kr_sbus_interface/config/gains.yaml new file mode 100644 index 00000000..0c57f8a8 --- /dev/null +++ b/interfaces/kr_sbus_interface/config/gains.yaml @@ -0,0 +1,15 @@ +gains: + pos: {x: 3.4, y: 3.4, z: 5.4} + vel: {x: 2.8, y: 2.8, z: 3.0} + ki: {x: 0.00, y: 0.00, z: 0.00, yaw: 0.01} + kib: {x: 0.00, y: 0.00, z: 0.00} + rot: {x: 1.5, y: 1.5, z: 1.0} + ang: {x: 0.13, y: 0.13, z: 0.1} + +corrections: + kf: 0.0e-08 + r: 0.0 + p: 0.0 + +max_pos_int: 0.5 +max_pos_int_b: 0.5 \ No newline at end of file diff --git a/interfaces/kr_sbus_interface/config/mav_manager_params.yaml b/interfaces/kr_sbus_interface/config/mav_manager_params.yaml new file mode 100644 index 00000000..c0c3c823 --- /dev/null +++ b/interfaces/kr_sbus_interface/config/mav_manager_params.yaml @@ -0,0 +1,6 @@ +server_wait_timeout: 0.5 +need_imu: false +need_output_data: true +use_attitude_safety_catch: true +max_attitude_angle: 0.43 +takeoff_height: 0.2 diff --git a/interfaces/kr_sbus_interface/config/neurofly.yaml b/interfaces/kr_sbus_interface/config/neurofly.yaml new file mode 100644 index 00000000..b7017a4e --- /dev/null +++ b/interfaces/kr_sbus_interface/config/neurofly.yaml @@ -0,0 +1,32 @@ +port_name: /dev/ttyTHS0 +enable_receiving_sbus_messages: true +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.66 # [kg] +disable_thrust_mapping: true +use_body_rates: false +# 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 +# when flying in angle mode +max_roll_rate: 1000.0 # [deg/s] +max_pitch_rate: 1000.0 # [deg/s] +max_yaw_rate: 1000.0 # [deg/s] +max_roll_angle: 50.0 # [deg] +max_pitch_angle: 50.0 # [deg] +alpha_vbat_filter: 0.01 +perform_thrust_voltage_compensation: false +thrust_ratio_voltage_map_a: -0.17044342 # [1/V] +thrust_ratio_voltage_map_b: 3.10495276 # [-] +n_lipo_cells: 3 # [-] diff --git a/interfaces/kr_sbus_interface/config/tracker_params.yaml b/interfaces/kr_sbus_interface/config/tracker_params.yaml new file mode 100644 index 00000000..fd97a0b0 --- /dev/null +++ b/interfaces/kr_sbus_interface/config/tracker_params.yaml @@ -0,0 +1,25 @@ +# This should contain tracker parameters + +line_tracker_distance: + default_v_des: 1.0 + default_a_des: 0.5 + epsilon: 0.1 + +line_tracker_min_jerk: + default_v_des: 1.0 + default_a_des: 0.5 + default_yaw_v_des: 0.8 + default_yaw_a_des: 0.2 + +trajectory_tracker: + max_vel_des: 0.5 + max_acc_des: 0.3 + +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_sbus_interface/config/trackers.yaml b/interfaces/kr_sbus_interface/config/trackers.yaml new file mode 100644 index 00000000..694f6177 --- /dev/null +++ b/interfaces/kr_sbus_interface/config/trackers.yaml @@ -0,0 +1,11 @@ +trackers: + - kr_trackers/LineTrackerMinJerk + - kr_trackers/LineTrackerDistance +# - kr_trackers/LineTrackerTrapezoid +# - kr_trackers/LineTrackerYaw + - kr_trackers/VelocityTrackerAction + - kr_trackers/NullTracker + - kr_trackers/CircleTracker + - kr_trackers/TrajectoryTracker + - kr_trackers/SmoothVelTracker + - kr_trackers/LissajousTracker diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h new file mode 100644 index 00000000..e0e5da02 --- /dev/null +++ b/interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h @@ -0,0 +1,18 @@ +#pragma once + +namespace sbus_bridge { + +namespace channel_mapping { + +static constexpr uint8_t kThrottle = 2; +static constexpr uint8_t kRoll = 0; +static constexpr uint8_t kPitch = 1; +static constexpr uint8_t kYaw = 3; +static constexpr uint8_t kArming = 4; +static constexpr uint8_t kKillSwitch = 7; +static constexpr uint8_t kControlMode = 5; +static constexpr uint8_t kGamepadMode = 6; + +} // namespace channel_mapping + +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h new file mode 100644 index 00000000..b4e86b99 --- /dev/null +++ b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h @@ -0,0 +1,149 @@ +#pragma once + +#include +#include "kr_sbus_interface/sbus_msg.h" +#include "kr_sbus_interface/sbus_serial_port.h" +#include +#include +#include + +#include +#include +#include +#include + +namespace sbus_bridge +{ + +enum class BridgeState +{ + OFF, + ARMING, + AUTONOMOUS_FLIGHT, + RC_FLIGHT, + KILL +}; + +class SBusBridge : public SBusSerialPort +{ + public: + SBusBridge(const rclcpp::Node::SharedPtr &node); + + void controlCommandCallback(const kr_mav_msgs::msg::SO3Command::ConstPtr &msg, const Eigen::Quaterniond &odom_q); + void armBridge(); + void disarmBridge(); + bool isBridgeArmed() const; + + virtual ~SBusBridge(); + + private: + void watchdogThread(); + + void handleReceivedSbusMessage(const SBusMsg &received_sbus_msg) override; + + SBusMsg generateSBusMessageFromSO3Command(const kr_mav_msgs::msg::SO3Command::ConstPtr &control_command, + const Eigen::Quaterniond &odom_q) const; + + void sendSBusMessageToSerialPort(const SBusMsg &sbus_msg); + + void setBridgeState(const BridgeState &desired_bridge_state); + + double thrust_model_kartik(double thrust) const; + + bool loadParameters(); + + // rclcpp Node + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + + // Mutex for: + // - bridge_state_ + // - control_mode_ + // - bridge_armed_ + // - time_last_active_control_command_received_ + // - time_last_rc_msg_received_ + // - arming_counter_ + // - time_last_sbus_msg_sent_ + // Also "setBridgeState" and "sendSBusMessageToSerialPort" should only be + // called when "main_mutex_" is locked + mutable std::mutex main_mutex_; + mutable std::mutex arm_mutex_; + // Mutex for: + // - battery_voltage_ + // - time_last_battery_voltage_received_ + // Also "generateSBusMessageFromControlCommand" should only be called when + // "battery_voltage_mutex_" is locked + mutable std::mutex battery_voltage_mutex_; + + // Subscribers + rclcpp::Subscription::SharedPtr control_command_sub_; + rclcpp::Subscription::SharedPtr battery_voltage_sub_; + // ROS 2: arm_bridge_sub_ removed unless you have a custom message/type for it + + // Timer + rclcpp::TimerBase::SharedPtr low_level_feedback_pub_timer_; + + // Watchdog + std::thread watchdog_thread_; + std::atomic_bool stop_watchdog_thread_; + rclcpp::Time time_last_rc_msg_received_; + rclcpp::Time time_last_sbus_msg_sent_; + rclcpp::Time time_last_battery_voltage_received_; + rclcpp::Time time_last_active_control_command_received_; + + BridgeState bridge_state_; + ControlMode control_mode_; + int arming_counter_; + double battery_voltage_; + + // Safety flags + bool bridge_armed_; + bool rc_was_disarmed_once_; + + std::atomic_bool destructor_invoked_; + + // Parameters + std::string port_name_; + bool enable_receiving_sbus_messages_; + + double control_command_timeout_; + double rc_timeout_; + + double mass_; + + bool disable_thrust_mapping_; + + double max_roll_rate_; + double max_pitch_rate_; + double max_yaw_rate_; + + double max_roll_angle_; + double max_pitch_angle_; + + double alpha_vbat_filter_; + bool perform_thrust_voltage_compensation_; + int n_lipo_cells_; + + // Constants + static constexpr double kLowLevelFeedbackPublishFrequency_ = 50.0; + + static constexpr int kSmoothingFailRepetitions_ = 5; + + static constexpr double kBatteryLowVoltagePerCell_ = 3.6; + static constexpr double kBatteryCriticalVoltagePerCell_ = 3.4; + static constexpr double kBatteryInvalidVoltagePerCell_ = 3.0; + static constexpr double kBatteryVoltageTimeout_ = 1.0; + + double thrust_vs_rpm_cof_a_; + double thrust_vs_rpm_cof_b_; + double thrust_vs_rpm_cof_c_; + double rpm_vs_throttle_linear_coeff_a_; + double rpm_vs_throttle_linear_coeff_b_; + double rpm_vs_throttle_quadratic_coeff_a_; + double rpm_vs_throttle_quadratic_coeff_b_; + double rpm_vs_throttle_quadratic_coeff_c_; + + bool use_body_rates_; +}; + +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h new file mode 100644 index 00000000..b6d411f3 --- /dev/null +++ b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h @@ -0,0 +1,61 @@ +#pragma once + +#include +#include + +namespace sbus_bridge { + +enum class ControlMode { NONE, ATTITUDE, BODY_RATES }; + +enum class ArmState { DISARMED, ARMED }; + +#pragma pack(push) +#pragma pack(1) +struct SBusMsg { + // Constants + static constexpr int kNChannels = 16; + static constexpr uint16_t kMinCmd = 174; // corresponds to 1000 on FC + static constexpr uint16_t kMeanCmd = 992; // corresponds to 1500 on FC + static constexpr uint16_t kMaxCmd = 1811; // corresponds to 2000 on FC + + rclcpp::Time timestamp; + + // Normal 11 bit channels + uint16_t channels[kNChannels]; + + // Digital channels (ch17 and ch18) + bool digital_channel_1; + bool digital_channel_2; + + // Flags + bool frame_lost; + bool failsafe; + + SBusMsg(); +// SBusMsg(const sbus_bridge::SbusRosMessage& sbus_ros_msg); + virtual ~SBusMsg(); + +// sbus_bridge::SbusRosMessage toRosMessage() const; + + void limitAllChannelsFeasible(); + void limitSbusChannelFeasible(const int channel_idx); + + // Setting sbus command helpers + void setThrottleCommand(const uint16_t throttle_cmd); + void setRollCommand(const uint16_t roll_cmd); + void setPitchCommand(const uint16_t pitch_cmd); + void setYawCommand(const uint16_t yaw_cmd); + void setControlMode(const ControlMode& control_mode); + void setControlModeAttitude(); + void setControlModeBodyRates(); + void setArmState(const ArmState& arm_state); + void setArmStateArmed(); + void setArmStateDisarmed(); + + // Sbus message check helpers + bool isArmed() const; + bool isKillSwitch() const; + ControlMode getControlMode() const; +}; +#pragma pack(pop) +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h new file mode 100644 index 00000000..4e8d3494 --- /dev/null +++ b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include + +#include "kr_sbus_interface/sbus_msg.h" + +namespace sbus_bridge { + +class SBusSerialPort { + public: + SBusSerialPort(); + SBusSerialPort(const std::string& port, + const bool start_receiver_thread); + virtual ~SBusSerialPort(); + + protected: + bool setUpSBusSerialPort(const std::string& port, + const bool start_receiver_thread); + + bool connectSerialPort(const std::string& port); + void disconnectSerialPort(); + + bool startReceiverThread(); + bool stopReceiverThread(); + + void transmitSerialSBusMessage(const SBusMsg& sbus_msg) const; + virtual void handleReceivedSbusMessage( + const sbus_bridge::SBusMsg& received_sbus_msg) = 0; + + private: + static constexpr int kSbusFrameLength_ = 25; + static constexpr uint8_t kSbusHeaderByte_ = 0x0F; + static constexpr uint8_t kSbusFooterByte_ = 0x00; + static constexpr int kPollTimeoutMilliSeconds_ = 500; + + bool configureSerialPortForSBus() const; + void serialPortReceiveThread(); + sbus_bridge::SBusMsg parseSbusMessage( + uint8_t sbus_msg_bytes[kSbusFrameLength_]) const; + + std::thread receiver_thread_; + std::atomic_bool receiver_thread_should_exit_; + + int serial_port_fd_; + rclcpp::Logger logger_ = rclcpp::get_logger("sbus_serial_port"); +}; + +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/thrust_mapping.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/thrust_mapping.h new file mode 100644 index 00000000..9faed5a9 --- /dev/null +++ b/interfaces/kr_sbus_interface/include/kr_sbus_interface/thrust_mapping.h @@ -0,0 +1,39 @@ +#pragma once + +#include + +namespace thrust_mapping { + +class CollectiveThrustMapping { + public: + CollectiveThrustMapping(); + CollectiveThrustMapping(const double thrust_map_a, const double thrust_map_b, + const double thrust_map_c, + const bool perform_thrust_voltage_compensation, + const double thrust_ratio_voltage_map_a, + const double thrust_ratio_voltage_map_b, + const int n_lipo_cells); + + virtual ~CollectiveThrustMapping(); + + uint16_t inverseThrustMapping(const double thrust, + const double battery_voltage) const; + + bool loadParameters(); + + private: + double thrust_map_a_; + double thrust_map_b_; + double thrust_map_c_; + + bool perform_thrust_voltage_compensation_; + double thrust_ratio_voltage_map_a_; + double thrust_ratio_voltage_map_b_; + int n_lipo_cells_; + + // Constants + static constexpr double kMinBatteryCompensationVoltagePerCell_ = 3.5; + static constexpr double kMaxBatteryCompensationVoltagePerCell_ = 4.2; +}; + +} // namespace thrust_mapping diff --git a/interfaces/kr_sbus_interface/launch/sbus_interface.py b/interfaces/kr_sbus_interface/launch/sbus_interface.py new file mode 100644 index 00000000..6cb8db49 --- /dev/null +++ b/interfaces/kr_sbus_interface/launch/sbus_interface.py @@ -0,0 +1,57 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + # Declare launch arguments + robot_ns = LaunchConfiguration('robot', default='/') + odom_topic = LaunchConfiguration('odom', default='odom') + so3_cmd_topic = LaunchConfiguration('so3_cmd', default='so3_cmd') + + # Path to the configuration file + config_file = os.path.join( + get_package_share_directory('kr_sbus_interface'), + 'config', + 'neurofly.yaml' + ) + + # Component container to load your SO3CmdToSbus component + sbus_container = ComposableNodeContainer( + name='so3cmd_to_sbus_container', + namespace=robot_ns, + package='rclcpp_components', + executable='component_container_mt', # multi-threaded + composable_node_descriptions=[ + ComposableNode( + package='kr_sbus_interface', + plugin='SO3CmdToSBUS', + name='so3cmd_to_sbus', + remappings=[ + ('odom', odom_topic), + ('so3_cmd', so3_cmd_topic), + ], + parameters=[config_file, {'port_name': '/dev/ttyTHS0'}], + ), + ], + output='screen', + ) + robot_arg = DeclareLaunchArgument( + 'robot', default_value='' + ) + odom_arg = DeclareLaunchArgument( + 'odom', default_value='odom' + ) + so3_cmd_arg = DeclareLaunchArgument( + 'so3_cmd', default_value='so3_cmd' + ) + return LaunchDescription([ + robot_arg, + odom_arg, + so3_cmd_arg, + sbus_container, + ]) diff --git a/interfaces/kr_sbus_interface/msg/SbusRosMessage.msg b/interfaces/kr_sbus_interface/msg/SbusRosMessage.msg new file mode 100644 index 00000000..3802e30f --- /dev/null +++ b/interfaces/kr_sbus_interface/msg/SbusRosMessage.msg @@ -0,0 +1,14 @@ +Header header + +# Regular 16 sbus channels with 11 bit value range each +uint16[16] channels + +# Digital channels +bool digital_channel_1 +bool digital_channel_2 + +# Frame lost flag +bool frame_lost + +# Failsafe flag +bool failsafe diff --git a/interfaces/kr_sbus_interface/nodelet_plugin.xml b/interfaces/kr_sbus_interface/nodelet_plugin.xml new file mode 100644 index 00000000..4c71b948 --- /dev/null +++ b/interfaces/kr_sbus_interface/nodelet_plugin.xml @@ -0,0 +1,7 @@ + + + + This take a so3_command and sends it to the robot using the SBUS Bridge + + + diff --git a/interfaces/kr_sbus_interface/package.xml b/interfaces/kr_sbus_interface/package.xml new file mode 100644 index 00000000..acc1e2a1 --- /dev/null +++ b/interfaces/kr_sbus_interface/package.xml @@ -0,0 +1,23 @@ + + + kr_sbus_interface + 0.0.0 + The kr_sbus_interface package + + Dexter Ong + + BSD + + ament_cmake + + rclcpp + nav_msgs + sensor_msgs + kr_mav_msgs + tf2 + roscpp + rclcpp_components + + ament_cmake + + diff --git a/interfaces/kr_sbus_interface/scripts/mavlink_telemetry.py b/interfaces/kr_sbus_interface/scripts/mavlink_telemetry.py new file mode 100644 index 00000000..6c70a881 --- /dev/null +++ b/interfaces/kr_sbus_interface/scripts/mavlink_telemetry.py @@ -0,0 +1,35 @@ +import rospy +from pymavlink import mavutil +from sensor_msgs.msg import BatteryState + +def mavlink_connect(port='/dev/ttyUSB0', baudrate=115200): + """Connect to a MAVLink source""" + return mavutil.mavlink_connection(port, baud=baudrate) + +def battery_status_publisher(): + rospy.init_node('battery_status_publisher', anonymous=True) + battery_pub = rospy.Publisher('battery_status', BatteryState, queue_size=10) + rate = rospy.Rate(100) + + # Connect to MAVLink (adjust port and baudrate as necessary) + mav = mavlink_connect(port='/dev/ttyUSB0', baudrate=115200) + + while not rospy.is_shutdown(): + message = mav.recv_match(blocking=True) + if message: + if message.get_type() == "SYS_STATUS": + battery_msg = BatteryState() + battery_msg.voltage = message.voltage_battery / 1000.0 / 4.0 + battery_msg.current = message.current_battery / 100.0 + battery_msg.percentage = message.battery_remaining + + battery_msg.header.stamp = rospy.Time.now() + battery_pub.publish(battery_msg) + + rate.sleep() + +if __name__ == '__main__': + try: + battery_status_publisher() + except rospy.ROSInterruptException: + pass diff --git a/interfaces/kr_sbus_interface/src/sbus_bridge.cpp b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp new file mode 100644 index 00000000..f8ad9648 --- /dev/null +++ b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp @@ -0,0 +1,662 @@ +#include + +#include + +#include + +// #include +#include + +#include + +namespace sbus_bridge { + + SBusBridge::SBusBridge(const rclcpp::Node::SharedPtr& node) + : node_(node), + stop_watchdog_thread_(false), + time_last_rc_msg_received_(), + time_last_sbus_msg_sent_(node_->now()), + time_last_battery_voltage_received_(node_->now()), + time_last_active_control_command_received_(), + bridge_state_(BridgeState::KILL), + control_mode_(ControlMode::NONE), + arming_counter_(0), + battery_voltage_(0.0), + bridge_armed_(false), + rc_was_disarmed_once_(false), + destructor_invoked_(false) { + + if (!loadParameters()) { + RCLCPP_ERROR(logger_, "Could not load parameters."); + rclcpp::shutdown(); + return; + } + +// sbus_cmd_pub_ = node_->create_publisher("sbus_cmd", 1); + + // Start serial port with receiver thread if receiving sbus messages is + // enabled + if (!setUpSBusSerialPort(port_name_, enable_receiving_sbus_messages_)) { + rclcpp::shutdown(); + return; + } + + // Start watchdog thread + try { + watchdog_thread_ = std::thread(&SBusBridge::watchdogThread, this); + } catch (...) { + RCLCPP_ERROR( + logger_, + "Could not successfully start watchdog thread. Exiting SBusBridge."); + rclcpp::shutdown(); + return; + } +} + +SBusBridge::~SBusBridge() { + destructor_invoked_ = true; + + // Stop SBus receiver thread + if (enable_receiving_sbus_messages_) { + stopReceiverThread(); + } + + // Stop watchdog thread + stop_watchdog_thread_ = true; + // Wait for watchdog thread to finish + watchdog_thread_.join(); + + // Now only one thread (the ROS thread) is remaining + + setBridgeState(BridgeState::OFF); + + // Send disarming SBus message for safety + // We repeat it to prevent any possible smoothing of commands on the flight + // controller to interfere with this + SBusMsg shut_down_message; + shut_down_message.setArmStateDisarmed(); + rclcpp::Rate loop_rate(110.0); + for (int i = 0; i < kSmoothingFailRepetitions_; i++) { + transmitSerialSBusMessage(shut_down_message); + loop_rate.sleep(); + } + + + // Close serial port + disconnectSerialPort(); +} + +void SBusBridge::watchdogThread() { + rclcpp::Rate watchdog_rate(110.0); + while (rclcpp::ok() && !stop_watchdog_thread_) { + watchdog_rate.sleep(); + + std::lock_guard main_lock(main_mutex_); + + const rclcpp::Time time_now = node_->now(); + + if (bridge_state_ == BridgeState::RC_FLIGHT && + (time_now - time_last_rc_msg_received_).seconds() > rc_timeout_) { + // If the last received RC command was armed but was received longer than + // rc_timeout ago we switch the bridge state to AUTONOMOUS_FLIGHT. + // In case there are no valid control commands the bridge state is set to + // OFF in the next check below + RCLCPP_WARN( + logger_, + "Remote control was active but no message from it was received " + "within timeout (%f s).", + rc_timeout_); + setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); + } + + // if (bridge_state_ == BridgeState::ARMING || + // bridge_state_ == BridgeState::AUTONOMOUS_FLIGHT) { + // if ((time_now - time_last_active_control_command_received_).seconds() > + // control_command_timeout_) { + // // When switching the bridge state to off, our watchdog ensures that a + // // disarming off message is repeated. + // RCLCPP_INFO(node_->get_logger(), "No active control command received, setting bridge state to OFF"); + // setBridgeState(BridgeState::OFF); + // // Note: Control could theoretically still be taken over by RC but if + // // this happened in flight it might require super human reaction since + // // in this case the quad can not be armed with non zero throttle by + // // the remote. + // } + // } + + if (bridge_state_ == BridgeState::KILL) { + // Send off message that disarms the vehicle + // We repeat it to prevent any weird behavior that occurs if the flight + // controller is not receiving commands for a while + SBusMsg kill_msg; + kill_msg.setArmStateDisarmed(); + sendSBusMessageToSerialPort(kill_msg); + // disarm bridge if needed + if (bridge_armed_) { + disarmBridge(); + } + } + + // Check battery voltage timeout + std::lock_guard battery_lock(battery_voltage_mutex_); + if ((time_now - time_last_battery_voltage_received_).seconds() > + kBatteryVoltageTimeout_) { + battery_voltage_ = 0.0; + if (perform_thrust_voltage_compensation_) { + RCLCPP_WARN_THROTTLE( + logger_, *node_->get_clock(), 1000, + "Can not perform battery voltage compensation because there " + "is no recent battery voltage measurement"); + } + } + + // Mutexes are unlocked because they go out of scope here + } +} + +void SBusBridge::handleReceivedSbusMessage(const SBusMsg &received_sbus_msg) +{ + { + std::lock_guard main_lock(main_mutex_); + + time_last_rc_msg_received_ = rclcpp::Clock().now(); + + if (received_sbus_msg.isKillSwitch()) + { + if (bridge_state_ != BridgeState::KILL) + { + setBridgeState(BridgeState::KILL); + RCLCPP_INFO(logger_, "Kill switch ON"); + } + } + else + { + if (bridge_state_ == BridgeState::KILL) + { + setBridgeState(BridgeState::OFF); + RCLCPP_WARN(logger_, "Kill switch OFF"); + } + } + + if (bridge_state_ == BridgeState::KILL) + { + // send disarm to FC to kill motors + // sendSBusMessageToSerialPort(received_sbus_msg); + + return; + } + + if (received_sbus_msg.isArmed()) + { + if (!rc_was_disarmed_once_) { + // This flag prevents that the vehicle can be armed if the RC is armed + // on startup of the bridge + RCLCPP_WARN_THROTTLE( + logger_, *node_->get_clock(), 1.0, + "RC needs to be disarmed once before it can take over control"); + return; + } + + // Immediately go into RC_FLIGHT state since RC always has priority + if (bridge_state_ != BridgeState::RC_FLIGHT) { + setBridgeState(BridgeState::RC_FLIGHT); + RCLCPP_INFO(logger_, "Control authority taken over by remote control."); + } + sendSBusMessageToSerialPort(received_sbus_msg); + control_mode_ = received_sbus_msg.getControlMode(); + } + else if (bridge_state_ == BridgeState::RC_FLIGHT) + { + // If the bridge was in state RC_FLIGHT and the RC is disarmed we set the + // state to AUTONOMOUS_FLIGHT + // In case there are valid control commands, the bridge will stay in + // AUTONOMOUS_FLIGHT, otherwise the watchdog will set the state to OFF + RCLCPP_INFO(logger_, "Control authority returned by remote control."); + if (bridge_armed_) + { + RCLCPP_INFO(logger_, "Bridge armed, setting bridge state to AUTONOMOUS_FLIGHT"); + setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); + } + else + { + // When switching the bridge state to off, our watchdog ensures that a + // disarming off message is sent + RCLCPP_INFO(logger_, "Bridge not armed, setting bridge state to OFF"); + setBridgeState(BridgeState::OFF); + } + } + else if (!rc_was_disarmed_once_) + { + RCLCPP_INFO( + logger_, + "Received disarmed RC message but RC was not disarmed once, ignoring it"); + rc_was_disarmed_once_ = true; + } + else if (bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) { + // not killed, not armed, not autonomous flight + sendSBusMessageToSerialPort(received_sbus_msg); + } + + // Main mutex is unlocked here because it goes out of scope + } + +// received_sbus_msg_pub_.publish(received_sbus_msg.toRosMessage()); +} + +void SBusBridge::controlCommandCallback( + const kr_mav_msgs::msg::SO3Command::ConstPtr &msg, + const Eigen::Quaterniond &odom_q) +{ + std::lock_guard main_lock(main_mutex_); + + if (destructor_invoked_) + { + // This ensures that if the destructor was invoked we do not try to write + // to the serial port anymore because of receiving a control command + return; + } + + // may need more logic @TODO + time_last_active_control_command_received_ = rclcpp::Clock().now(); + + if (!bridge_armed_ || bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) + { + // If bridge is not armed we do not allow control commands to be sent + // RC has priority over control commands for autonomous flying + if (!bridge_armed_ && msg->aux.enable_motors && + bridge_state_ != BridgeState::RC_FLIGHT) + { + RCLCPP_WARN( + logger_, + "Received active control command but sbus bridge is not armed. " + "Please arm the bridge before sending control commands."); + } + return; + } + + SBusMsg sbus_msg_to_send; + { + std::lock_guard battery_lock(battery_voltage_mutex_); + // Set commands + sbus_msg_to_send = generateSBusMessageFromSO3Command(msg, odom_q); + // Battery voltage mutex is unlocked because it goes out of scope here + } + + // Send disarm if necessary + if (!msg->aux.enable_motors) + { + if (bridge_state_ == BridgeState::ARMING || + bridge_state_ == BridgeState::AUTONOMOUS_FLIGHT) + { + RCLCPP_INFO(logger_, "Control command received, setting bridge state to OFF"); + setBridgeState(BridgeState::OFF); + } + // Make sure vehicle is disarmed to immediately switch it off + sbus_msg_to_send.setArmStateDisarmed(); + } + + // Limit channels + sbus_msg_to_send.limitAllChannelsFeasible(); + + // Immediately send SBus message + sendSBusMessageToSerialPort(sbus_msg_to_send); + + // Main mutex is unlocked because it goes out of scope here +} + +void SBusBridge::sendSBusMessageToSerialPort(const SBusMsg& sbus_msg) +{ + // // print bridge state + // switch (bridge_state_) { + // case BridgeState::OFF: + // ROS_INFO("Bridge state: OFF"); + // break; + // case BridgeState::ARMING: + // ROS_INFO("Bridge state: ARMING"); + // break; + // case BridgeState::AUTONOMOUS_FLIGHT: + // ROS_INFO("Bridge state: AUTONOMOUS_FLIGHT"); + // break; + // case BridgeState::RC_FLIGHT: + // ROS_INFO("Bridge state: RC_FLIGHT"); + // break; + // default: + // ROS_INFO("Bridge state: UNKNOWN"); + // } + SBusMsg sbus_message_to_send = sbus_msg; + + switch (bridge_state_) { + case BridgeState::OFF: + // Disarm vehicle + sbus_message_to_send.setArmStateDisarmed(); + // ROS_INFO("Bridge state: OFF"); + break; + + case BridgeState::ARMING: + // Since there might be some RC commands smoothing and checks on multiple + // messages before arming, we repeat the messages with minimum throttle + // and arming command multiple times. 5 times seems to work robustly. + if (arming_counter_ >= kSmoothingFailRepetitions_) { + setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); + } else { + // Set thrust to minimum command to make sure FC arms + sbus_message_to_send.setThrottleCommand(SBusMsg::kMinCmd); + sbus_message_to_send.setArmStateArmed(); + arming_counter_++; + } + break; + + case BridgeState::AUTONOMOUS_FLIGHT: + sbus_message_to_send.setArmStateArmed(); + if (use_body_rates_) + { + sbus_message_to_send.setControlModeBodyRates(); + } else + { + sbus_message_to_send.setControlModeAttitude(); + } + break; + + case BridgeState::RC_FLIGHT: + // Passing RC command straight through + // ROS_INFO("RC MODE: throttle_cmd %d", sbus_message_to_send.channels[sbus_bridge::channel_mapping::kThrottle]); + sbus_message_to_send.setControlModeAttitude(); + break; + + case BridgeState::KILL: + // Disarm vehicle + sbus_message_to_send.setArmStateDisarmed(); + // ROS_INFO("Bridge state: OFF"); + break; + + default: + // Disarm the vehicle because this code must be terribly wrong + sbus_message_to_send.setArmStateDisarmed(); + RCLCPP_WARN( + logger_, "Bridge is in unknown state, vehicle will be disarmed"); + break; + } + + if ((rclcpp::Clock().now() - time_last_sbus_msg_sent_).seconds() <= 0.006) { + // An SBUS message takes 3ms to be transmitted by the serial port so let's + // not stress it too much. This should only happen in case of switching + // between control commands and rc commands + if (bridge_state_ == BridgeState::ARMING) { + // In case of arming we want to send kSmoothingFailRepetitions_ messages + // with minimum throttle to the flight controller. Since this check + // prevents the message from being sent out we reduce the counter that + // was incremented above assuming the message would actually be sent. + arming_counter_--; + } + return; + } + + sbus_message_to_send.timestamp = rclcpp::Clock().now(); + transmitSerialSBusMessage(sbus_message_to_send); + time_last_sbus_msg_sent_ = rclcpp::Clock().now(); +} + +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)); +} + +double SBusBridge::thrust_model_kartik(double thrust) const +{ + int num_props = 4; + double avg_thrust = std::max(0.0, thrust) / num_props; + + // Scale thrust to individual rotor velocities (RPM) + 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 betaflight + double throttle = + (omega_avg - rpm_vs_throttle_linear_coeff_b_) / rpm_vs_throttle_linear_coeff_a_; + return throttle; +} + +SBusMsg SBusBridge::generateSBusMessageFromSO3Command(const kr_mav_msgs::msg::SO3Command::ConstPtr& msg, const Eigen::Quaterniond& odom_q) const +{ + SBusMsg sbus_msg; + + // set sbus_msg to not killed + sbus_msg.channels[sbus_bridge::channel_mapping::kKillSwitch] = 1792; + + sbus_msg.setArmStateArmed(); + + // 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); + // const Eigen::Vector3d ang_vel(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); + + // convert to tf::Quaternion + // tf::Quaternion imu_tf = tf::Quaternion(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w()); + // tf::Quaternion odom_tf = tf::Quaternion(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w()); + + const Eigen::Matrix3d R_cur(odom_q); + + double thrust = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); + + double throttle = 0.0; + if (thrust > 1e-5) + { + throttle = thrust_model_kartik(thrust); + } + + // remap throttle (1000 to 2000) to sbus (kMinCmd to kMaxCmd) + uint16_t throttle_cmd = round(((throttle - 1000) / 1000) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); + RCLCPP_INFO_THROTTLE( + logger_, *node_->get_clock(), 1.0, + "AUTONOMOUS MODE: thrust: %f throttle: %f throttle_cmd: %d", + thrust, throttle, throttle_cmd); + sbus_msg.setThrottleCommand(throttle_cmd); + + // convert quaternion to euler + Eigen::Vector3d desired_euler_angles = q_des.toRotationMatrix().eulerAngles(0, 1, 2); + + // sanity check if larger than 3 rad, set to 0 + for (int i = 0; i < 3; i++) { + if (desired_euler_angles(i) > 3.0 || desired_euler_angles(i) < -3.0) + { + desired_euler_angles(i) = 0; + } + } + + // parse commands + uint16_t roll_cmd, pitch_cmd, yaw_cmd; + + if (use_body_rates_) + { + // get body rates + double roll_rate = msg->angular_velocity.x; + double pitch_rate = msg->angular_velocity.y; + + roll_rate = std::max(-max_roll_rate_, std::min(max_roll_rate_, roll_rate)); + pitch_rate = std::max(-max_pitch_rate_, std::min(max_pitch_rate_, pitch_rate)); + + // additional safety check + roll_rate = std::max(-400.0, std::min(400.0, roll_rate)); + pitch_rate = std::max(-400.0, std::min(400.0, pitch_rate)); + + roll_cmd = round((roll_rate + max_roll_rate_) / (2 * max_roll_rate_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); + pitch_cmd = round((pitch_rate + max_pitch_rate_) / (2 * max_pitch_rate_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); + + RCLCPP_INFO_THROTTLE( + logger_, *node_->get_clock(), 1.0, + "AUTONOMOUS MODE: roll_rate: %f pitch_rate: %f roll_cmd: %d pitch_cmd: %d", + roll_rate, pitch_rate, roll_cmd, pitch_cmd); + } + else + { + // get body angles + double yaw, roll, pitch; + // use tf2 to convert quaternion to euler + tf2::Matrix3x3(tf2::Quaternion(q_des.x(), q_des.y(), q_des.z(), q_des.w())).getRPY(roll, pitch, yaw); + // convert to degrees + double roll_deg = roll * 180.0 / M_PI; + double pitch_deg = pitch * 180.0 / M_PI; + + // clip max roll + roll_deg = std::max(-max_roll_angle_, std::min(max_roll_angle_, roll_deg)); + // clip max pitch + pitch_deg = std::max(-max_pitch_angle_, std::min(max_pitch_angle_, pitch_deg)); + + roll_cmd = round((roll_deg + max_roll_angle_) / (2 * max_roll_angle_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); + pitch_cmd = round((pitch_deg + max_pitch_angle_) / (2 * max_pitch_angle_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); + + RCLCPP_INFO_THROTTLE( + logger_, *node_->get_clock(), 1.0, + "AUTONOMOUS MODE: roll_deg: %f pitch_deg: %f roll_cmd: %d pitch_cmd: %d", + roll_deg, pitch_deg, roll_cmd, pitch_cmd); + } + + // always use yaw rate + double yaw_rate = -msg->angular_velocity.z; // flip sign since FC yaw is inverted (z-down) + yaw_rate = yaw_rate * 180.0 / M_PI; // convert to degrees + yaw_rate = std::max(-400.0, std::min(400.0, yaw_rate)); // clip max yaw rate + yaw_cmd = round((yaw_rate + max_yaw_rate_) / (2 * max_yaw_rate_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); + + RCLCPP_INFO_THROTTLE( + logger_, *node_->get_clock(), 1.0, + "AUTONOMOUS MODE: yaw_rate: %f yaw_cmd: %d", + yaw_rate, yaw_cmd); + + sbus_msg.setRollCommand(roll_cmd); + sbus_msg.setPitchCommand(pitch_cmd); + sbus_msg.setYawCommand(yaw_cmd); + +// // publish values as odom message +// nav_msgs::Odometry sbus_cmd; +// sbus_cmd.header.stamp = ros::Time::now(); +// // convert to float +// sbus_cmd.pose.pose.position.x = static_cast(roll_cmd); +// sbus_cmd.pose.pose.position.y = static_cast(pitch_cmd); +// sbus_cmd.pose.pose.position.z = static_cast(yaw_cmd); + +// // publish +// sbus_cmd_pub_.publish(sbus_cmd); + +// ROS_INFO("Thrust: %f", thrust_mapping_.inverseThrustMapping( +// thrust * mass_, battery_voltage_)); + // ROS_INFO("Roll: %f", round((desired_euler_angles(0) / max_roll_angle_) * + // (SBusMsg::kMaxCmd - SBusMsg::kMeanCmd) + + // SBusMsg::kMeanCmd)); + // ROS_INFO("Pitch: %f", round((desired_euler_angles(1) / max_pitch_angle_) * + // (SBusMsg::kMaxCmd - SBusMsg::kMeanCmd) + + // SBusMsg::kMeanCmd)); + // ROS_INFO("Yaw: %f", round((msg->angular_velocity.z / max_yaw_rate_) * + // (SBusMsg::kMaxCmd - SBusMsg::kMeanCmd) + + // SBusMsg::kMeanCmd)); + // ROS_INFO("Desired Euler Angles: %f, %f, %f", desired_euler_angles(0), desired_euler_angles(1), desired_euler_angles(2)); + // ROS_INFO("Current Euler Angles: %f, %f, %f", odom_q.toRotationMatrix().eulerAngles(0, 1, 2)(0), odom_q.toRotationMatrix().eulerAngles(0, 1, 2)(1), odom_q.toRotationMatrix().eulerAngles(0, 1, 2)(2)); + return sbus_msg; +} + +void SBusBridge::setBridgeState(const BridgeState& desired_bridge_state) { + switch (desired_bridge_state) { + case BridgeState::OFF: + bridge_state_ = desired_bridge_state; + break; + + case BridgeState::ARMING: + bridge_state_ = desired_bridge_state; + arming_counter_ = 0; + break; + + case BridgeState::AUTONOMOUS_FLIGHT: + bridge_state_ = desired_bridge_state; + break; + + case BridgeState::RC_FLIGHT: + bridge_state_ = desired_bridge_state; + break; + + case BridgeState::KILL: + bridge_state_ = desired_bridge_state; + break; + + default: + RCLCPP_WARN( + logger_, "Wanted to switch to unknown bridge state, setting to OFF"); + bridge_state_ = BridgeState::OFF; + } +} + +void SBusBridge::armBridge() +{ + std::lock_guard main_lock(arm_mutex_); + bridge_armed_ = true; +} + +void SBusBridge::disarmBridge() +{ + std::lock_guard main_lock(arm_mutex_); + bridge_armed_ = false; +} + +bool SBusBridge::isBridgeArmed() const +{ + return bridge_armed_; +} + +bool SBusBridge::loadParameters() +{ + node_->declare_parameter("port_name", std::string("/dev/ttyUSB0")); + node_->declare_parameter("enable_receiving_sbus_messages", true); + node_->declare_parameter("control_command_timeout", 0.5); + node_->declare_parameter("rc_timeout", 0.5); + node_->declare_parameter("mass", 1.0); + node_->declare_parameter("disable_thrust_mapping", false); + node_->declare_parameter("max_roll_rate", 400.0); + node_->declare_parameter("max_pitch_rate", 400.0); + node_->declare_parameter("max_yaw_rate", 400.0); + node_->declare_parameter("max_roll_angle", 0.5); + node_->declare_parameter("max_pitch_angle", 0.5); + node_->declare_parameter("alpha_vbat_filter", 0.1); + node_->declare_parameter("perform_thrust_voltage_compensation", true); + node_->declare_parameter("n_lipo_cells", 3); + node_->declare_parameter("thrust_vs_rpm_cof_a_", 0.0001); + node_->declare_parameter("thrust_vs_rpm_cof_b_", 0.0); + node_->declare_parameter("thrust_vs_rpm_cof_c_", 0.0); + node_->declare_parameter("rpm_vs_throttle_linear_coeff_a_", 0.0001); + node_->declare_parameter("rpm_vs_throttle_linear_coeff_b_", 0.0); + node_->declare_parameter("rpm_vs_throttle_quadratic_coeff_a_", 0.0001); + node_->declare_parameter("rpm_vs_throttle_quadratic_coeff_b_", 0.0); + node_->declare_parameter("rpm_vs_throttle_quadratic_coeff_c_", 0.0); + node_->declare_parameter("use_body_rates", false); + + // Get parameters + if (!node_->get_parameter("port_name", port_name_) || + !node_->get_parameter("enable_receiving_sbus_messages", enable_receiving_sbus_messages_) || + !node_->get_parameter("control_command_timeout", control_command_timeout_) || + !node_->get_parameter("rc_timeout", rc_timeout_) || + !node_->get_parameter("mass", mass_) || + !node_->get_parameter("disable_thrust_mapping", disable_thrust_mapping_) || + !node_->get_parameter("max_roll_rate", max_roll_rate_) || + !node_->get_parameter("max_pitch_rate", max_pitch_rate_) || + !node_->get_parameter("max_yaw_rate", max_yaw_rate_) || + !node_->get_parameter("max_roll_angle", max_roll_angle_) || + !node_->get_parameter("max_pitch_angle", max_pitch_angle_) || + !node_->get_parameter("alpha_vbat_filter", alpha_vbat_filter_) || + !node_->get_parameter("perform_thrust_voltage_compensation", perform_thrust_voltage_compensation_) || + !node_->get_parameter("n_lipo_cells", n_lipo_cells_) || + !node_->get_parameter("thrust_vs_rpm_cof_a_", thrust_vs_rpm_cof_a_) || + !node_->get_parameter("thrust_vs_rpm_cof_b_", thrust_vs_rpm_cof_b_) || + !node_->get_parameter("thrust_vs_rpm_cof_c_", thrust_vs_rpm_cof_c_) || + !node_->get_parameter("rpm_vs_throttle_linear_coeff_a_", rpm_vs_throttle_linear_coeff_a_) || + !node_->get_parameter("rpm_vs_throttle_linear_coeff_b_", rpm_vs_throttle_linear_coeff_b_) || + !node_->get_parameter("rpm_vs_throttle_quadratic_coeff_a_", rpm_vs_throttle_quadratic_coeff_a_) || + !node_->get_parameter("rpm_vs_throttle_quadratic_coeff_b_", rpm_vs_throttle_quadratic_coeff_b_) || + !node_->get_parameter("rpm_vs_throttle_quadratic_coeff_c_", rpm_vs_throttle_quadratic_coeff_c_) || + !node_->get_parameter("use_body_rates", use_body_rates_)) { + return false; + } + return true; +} + +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/src/sbus_msg.cpp b/interfaces/kr_sbus_interface/src/sbus_msg.cpp new file mode 100644 index 00000000..fea65423 --- /dev/null +++ b/interfaces/kr_sbus_interface/src/sbus_msg.cpp @@ -0,0 +1,112 @@ +#include "kr_sbus_interface/sbus_msg.h" + +#include "kr_sbus_interface/channel_mapping.h" + +namespace sbus_bridge { + +SBusMsg::SBusMsg() + : timestamp(rclcpp::Clock().now()), + digital_channel_1(false), + digital_channel_2(false), + frame_lost(false), + failsafe(false) { + for (int i = 0; i < kNChannels; i++) { + channels[i] = kMeanCmd; + } +} + +SBusMsg::~SBusMsg() {} + +void SBusMsg::limitAllChannelsFeasible() { + for (uint8_t i = 0; i < kNChannels; i++) { + limitSbusChannelFeasible(i); + } +} + +void SBusMsg::limitSbusChannelFeasible(const int channel_idx) { + if (channel_idx < 0 || channel_idx >= kNChannels) { + return; + } + + if (channels[channel_idx] > kMaxCmd) { + channels[channel_idx] = kMaxCmd; + } + if (channels[channel_idx] < kMinCmd) { + channels[channel_idx] = kMinCmd; + } +} + +void SBusMsg::setThrottleCommand(const uint16_t throttle_cmd) { + channels[channel_mapping::kThrottle] = throttle_cmd; +} + +void SBusMsg::setRollCommand(const uint16_t roll_cmd) { + channels[channel_mapping::kRoll] = roll_cmd; +} + +void SBusMsg::setPitchCommand(const uint16_t pitch_cmd) { + channels[channel_mapping::kPitch] = pitch_cmd; +} + +void SBusMsg::setYawCommand(const uint16_t yaw_cmd) { + channels[channel_mapping::kYaw] = yaw_cmd; +} + +void SBusMsg::setControlMode(const ControlMode& control_mode) { + if (control_mode == ControlMode::ATTITUDE) { + channels[channel_mapping::kControlMode] = kMinCmd; + } else if (control_mode == ControlMode::BODY_RATES) { + channels[channel_mapping::kControlMode] = kMaxCmd; + } +} + +void SBusMsg::setControlModeAttitude() { + setControlMode(ControlMode::ATTITUDE); +} + +void SBusMsg::setControlModeBodyRates() { + setControlMode(ControlMode::BODY_RATES); +} + +void SBusMsg::setArmState(const ArmState& arm_state) { + if (arm_state == ArmState::ARMED) { + channels[channel_mapping::kArming] = kMaxCmd; + } else { + channels[channel_mapping::kArming] = kMinCmd; + } +} + +void SBusMsg::setArmStateArmed() { setArmState(ArmState::ARMED); } + +void SBusMsg::setArmStateDisarmed() { + setArmState(ArmState::DISARMED); + // Should not be necessary but for safety we also set the throttle command + // to the minimum + setThrottleCommand(kMinCmd); +} + +bool SBusMsg::isArmed() const { + if (channels[channel_mapping::kArming] <= kMeanCmd) { + return false; + } + + return true; +} + +bool SBusMsg::isKillSwitch() const { + if (channels[channel_mapping::kKillSwitch] > kMeanCmd) { + return false; + } + + return true; +} + +ControlMode SBusMsg::getControlMode() const { + if (channels[channel_mapping::kControlMode] > kMeanCmd) { + return ControlMode::BODY_RATES; + } + + return ControlMode::ATTITUDE; +} + +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp b/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp new file mode 100644 index 00000000..a8a573cd --- /dev/null +++ b/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp @@ -0,0 +1,398 @@ +#include "kr_sbus_interface/sbus_serial_port.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace sbus_bridge { + +SBusSerialPort::SBusSerialPort() + : receiver_thread_should_exit_(false), + serial_port_fd_(-1) { + // Optional: Initialize with default values + logger_ = rclcpp::get_logger("sbus_serial_port"); +} + +SBusSerialPort::SBusSerialPort(const std::string& port, + const bool start_receiver_thread) + : receiver_thread_(), + receiver_thread_should_exit_(false), + serial_port_fd_(-1) { + setUpSBusSerialPort(port, start_receiver_thread); + logger_ = rclcpp::get_logger("sbus_serial_port"); +} + +SBusSerialPort::~SBusSerialPort() { disconnectSerialPort(); } + +bool SBusSerialPort::setUpSBusSerialPort(const std::string& port, + const bool start_receiver_thread) { + if (!connectSerialPort(port)) { + return false; + } + + if (start_receiver_thread) { + if (!startReceiverThread()) { + return false; + } + } + + return true; +} + +bool SBusSerialPort::connectSerialPort(const std::string& port) { + // Open serial port + // O_RDWR - Read and write + // O_NOCTTY - Ignore special chars like CTRL-C + serial_port_fd_ = open(port.c_str(), O_RDWR | O_NOCTTY); + + if (serial_port_fd_ == -1) { + RCLCPP_ERROR(logger_, "Could not open serial port %s", port.c_str()); + return false; + } + + if (!configureSerialPortForSBus()) { + close(serial_port_fd_); + RCLCPP_ERROR(logger_, "Could not set necessary configuration of serial port"); + return false; + } + + RCLCPP_INFO(logger_, "Connected to serial port %s", port.c_str()); + return true; +} + +void SBusSerialPort::disconnectSerialPort() { + stopReceiverThread(); + close(serial_port_fd_); +} + +bool SBusSerialPort::startReceiverThread() { + // Start watchdog thread + try { + receiver_thread_ = + std::thread(&SBusSerialPort::serialPortReceiveThread, this); + } catch (...) { + RCLCPP_ERROR(logger_, "Could not successfully start SBUS receiver thread."); + return false; + } + + return true; +} + +bool SBusSerialPort::stopReceiverThread() { + if (!receiver_thread_.joinable()) { + return true; + } + + receiver_thread_should_exit_ = true; + + // Wait for receiver thread to finish + receiver_thread_.join(); + + return true; +} + +bool SBusSerialPort::configureSerialPortForSBus() const { + // clear config + fcntl(serial_port_fd_, F_SETFL, 0); + // read non blocking + fcntl(serial_port_fd_, F_SETFL, FNDELAY); + + struct termios2 uart_config; + /* Fill the struct for the new configuration */ + ioctl(serial_port_fd_, TCGETS2, &uart_config); + + // Output flags - Turn off output processing + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + + // Input flags - Turn off input processing + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= + ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + + // + // No line processing: + // echo off + // echo newline off + // canonical mode off, + // extended input processing off + // signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + // Turn off character processing + // Turn off odd parity + uart_config.c_cflag &= ~(CSIZE | PARODD | CBAUD); + + // Enable parity generation on output and parity checking for input. + uart_config.c_cflag |= PARENB; + // Set two stop bits, rather than one. + uart_config.c_cflag |= CSTOPB; + // No output processing, force 8 bit input + uart_config.c_cflag |= CS8; + // Enable a non standard baud rate + uart_config.c_cflag |= BOTHER; + + // Set custom baud rate of 100'000 bits/s necessary for sbus + const speed_t spd = 100000; + uart_config.c_ispeed = spd; + uart_config.c_ospeed = spd; + + if (ioctl(serial_port_fd_, TCSETS2, &uart_config) < 0) { + RCLCPP_ERROR(logger_, "Could not set configuration of serial port"); + return false; + } + + return true; +} + +void SBusSerialPort::transmitSerialSBusMessage(const SBusMsg& sbus_msg) const { + static uint8_t buffer[kSbusFrameLength_]; + + // SBUS header + buffer[0] = kSbusHeaderByte_; + + // 16 channels of 11 bit data + buffer[1] = (uint8_t)((sbus_msg.channels[0] & 0x07FF)); + buffer[2] = (uint8_t)((sbus_msg.channels[0] & 0x07FF) >> 8 | + (sbus_msg.channels[1] & 0x07FF) << 3); + buffer[3] = (uint8_t)((sbus_msg.channels[1] & 0x07FF) >> 5 | + (sbus_msg.channels[2] & 0x07FF) << 6); + buffer[4] = (uint8_t)((sbus_msg.channels[2] & 0x07FF) >> 2); + buffer[5] = (uint8_t)((sbus_msg.channels[2] & 0x07FF) >> 10 | + (sbus_msg.channels[3] & 0x07FF) << 1); + buffer[6] = (uint8_t)((sbus_msg.channels[3] & 0x07FF) >> 7 | + (sbus_msg.channels[4] & 0x07FF) << 4); + buffer[7] = (uint8_t)((sbus_msg.channels[4] & 0x07FF) >> 4 | + (sbus_msg.channels[5] & 0x07FF) << 7); + buffer[8] = (uint8_t)((sbus_msg.channels[5] & 0x07FF) >> 1); + buffer[9] = (uint8_t)((sbus_msg.channels[5] & 0x07FF) >> 9 | + (sbus_msg.channels[6] & 0x07FF) << 2); + buffer[10] = (uint8_t)((sbus_msg.channels[6] & 0x07FF) >> 6 | + (sbus_msg.channels[7] & 0x07FF) << 5); + buffer[11] = (uint8_t)((sbus_msg.channels[7] & 0x07FF) >> 3); + buffer[12] = (uint8_t)((sbus_msg.channels[8] & 0x07FF)); + buffer[13] = (uint8_t)((sbus_msg.channels[8] & 0x07FF) >> 8 | + (sbus_msg.channels[9] & 0x07FF) << 3); + buffer[14] = (uint8_t)((sbus_msg.channels[9] & 0x07FF) >> 5 | + (sbus_msg.channels[10] & 0x07FF) << 6); + buffer[15] = (uint8_t)((sbus_msg.channels[10] & 0x07FF) >> 2); + buffer[16] = (uint8_t)((sbus_msg.channels[10] & 0x07FF) >> 10 | + (sbus_msg.channels[11] & 0x07FF) << 1); + buffer[17] = (uint8_t)((sbus_msg.channels[11] & 0x07FF) >> 7 | + (sbus_msg.channels[12] & 0x07FF) << 4); + buffer[18] = (uint8_t)((sbus_msg.channels[12] & 0x07FF) >> 4 | + (sbus_msg.channels[13] & 0x07FF) << 7); + buffer[19] = (uint8_t)((sbus_msg.channels[13] & 0x07FF) >> 1); + buffer[20] = (uint8_t)((sbus_msg.channels[13] & 0x07FF) >> 9 | + (sbus_msg.channels[14] & 0x07FF) << 2); + buffer[21] = (uint8_t)((sbus_msg.channels[14] & 0x07FF) >> 6 | + (sbus_msg.channels[15] & 0x07FF) << 5); + buffer[22] = (uint8_t)((sbus_msg.channels[15] & 0x07FF) >> 3); + + // SBUS flags + // (bit0 = least significant bit) + // bit0 = ch17 = digital channel (0x01) + // bit1 = ch18 = digital channel (0x02) + // bit2 = Frame lost, equivalent red LED on receiver (0x04) + // bit3 = Failsafe activated (0x08) + // bit4 = n/a + // bit5 = n/a + // bit6 = n/a + // bit7 = n/a + buffer[23] = 0x00; + if (sbus_msg.digital_channel_1) { + buffer[23] |= 0x01; + } + if (sbus_msg.digital_channel_2) { + buffer[23] |= 0x02; + } + if (sbus_msg.frame_lost) { + buffer[23] |= 0x04; + } + if (sbus_msg.failsafe) { + buffer[23] |= 0x08; + } + + // SBUS footer + buffer[24] = kSbusFooterByte_; + + const int written = write(serial_port_fd_, (char*)buffer, kSbusFrameLength_); + // tcflush(serial_port_fd_, TCOFLUSH); // There were rumors that this might + // not work on Odroids... + if (written != kSbusFrameLength_) { + RCLCPP_ERROR(logger_, "Wrote %d bytes but should have written %d", written, kSbusFrameLength_); + } +} + +void SBusSerialPort::serialPortReceiveThread() { + struct pollfd fds[1]; + fds[0].fd = serial_port_fd_; + fds[0].events = POLLIN; + + uint8_t init_buf[10]; + while (read(serial_port_fd_, init_buf, sizeof(init_buf)) > 0) { + // On startup, as long as we receive something, we keep reading to ensure + // that the first byte of the first poll is the start of an SBUS message + // and not some arbitrary byte. + // This should help to get the framing in sync in the beginning. + usleep(100); + } + + std::deque bytes_buf; + + while (!receiver_thread_should_exit_) { + // Buffer to read bytes from serial port. We make it large enough to + // potentially contain 4 sbus messages but its actual size probably does + // not matter too much + uint8_t read_buf[4 * kSbusFrameLength_]; + + if (poll(fds, 1, kPollTimeoutMilliSeconds_) > 0) { + if (fds[0].revents & POLLIN) { + const ssize_t nread = read(serial_port_fd_, read_buf, sizeof(read_buf)); + + for (ssize_t i = 0; i < nread; i++) { + bytes_buf.push_back(read_buf[i]); + } + + bool valid_sbus_message_received = false; + uint8_t sbus_msg_bytes[kSbusFrameLength_]; + while (bytes_buf.size() >= kSbusFrameLength_) { + // Check if we have a potentially valid SBUS message + // A valid SBUS message must have to correct header and footer byte + // as well as zeros in the four most significant bytes of the flag + // byte (byte 23) + if (bytes_buf.front() == kSbusHeaderByte_ && + !(bytes_buf[kSbusFrameLength_ - 2] & 0xF0) && + bytes_buf[kSbusFrameLength_ - 1] == kSbusFooterByte_) { + for (uint8_t i = 0; i < kSbusFrameLength_; i++) { + sbus_msg_bytes[i] = bytes_buf.front(); + bytes_buf.pop_front(); + } + + valid_sbus_message_received = true; + } else { + // If it is not a valid SBUS message but has a correct header byte + // we need to pop it to prevent staying in this loop forever + bytes_buf.pop_front(); + RCLCPP_WARN(logger_, "SBUS message framing not in sync"); + } + + // If not, pop front elements until we have a valid header byte + while (!bytes_buf.empty() && bytes_buf.front() != kSbusHeaderByte_) { + bytes_buf.pop_front(); + } + } + + if (valid_sbus_message_received) { + // Sometimes we read more than one sbus message at the same time + // By running the loop above for as long as possible before handling + // the received sbus message we achieve to only process the latest one + const SBusMsg received_sbus_msg = parseSbusMessage(sbus_msg_bytes); + handleReceivedSbusMessage(received_sbus_msg); + } + } + } + } + + return; +} + +SBusMsg SBusSerialPort::parseSbusMessage( + uint8_t sbus_msg_bytes[kSbusFrameLength_]) const { + SBusMsg sbus_msg; + + sbus_msg.timestamp = rclcpp::Clock().now(); + + // Decode the 16 regular channels + sbus_msg.channels[0] = + (((uint16_t)sbus_msg_bytes[1]) | ((uint16_t)sbus_msg_bytes[2] << 8)) & + 0x07FF; + sbus_msg.channels[1] = (((uint16_t)sbus_msg_bytes[2] >> 3) | + ((uint16_t)sbus_msg_bytes[3] << 5)) & + 0x07FF; + sbus_msg.channels[2] = + (((uint16_t)sbus_msg_bytes[3] >> 6) | ((uint16_t)sbus_msg_bytes[4] << 2) | + ((uint16_t)sbus_msg_bytes[5] << 10)) & + 0x07FF; + sbus_msg.channels[3] = (((uint16_t)sbus_msg_bytes[5] >> 1) | + ((uint16_t)sbus_msg_bytes[6] << 7)) & + 0x07FF; + sbus_msg.channels[4] = (((uint16_t)sbus_msg_bytes[6] >> 4) | + ((uint16_t)sbus_msg_bytes[7] << 4)) & + 0x07FF; + sbus_msg.channels[5] = + (((uint16_t)sbus_msg_bytes[7] >> 7) | ((uint16_t)sbus_msg_bytes[8] << 1) | + ((uint16_t)sbus_msg_bytes[9] << 9)) & + 0x07FF; + sbus_msg.channels[6] = (((uint16_t)sbus_msg_bytes[9] >> 2) | + ((uint16_t)sbus_msg_bytes[10] << 6)) & + 0x07FF; + sbus_msg.channels[7] = (((uint16_t)sbus_msg_bytes[10] >> 5) | + ((uint16_t)sbus_msg_bytes[11] << 3)) & + 0x07FF; + sbus_msg.channels[8] = + (((uint16_t)sbus_msg_bytes[12]) | ((uint16_t)sbus_msg_bytes[13] << 8)) & + 0x07FF; + sbus_msg.channels[9] = (((uint16_t)sbus_msg_bytes[13] >> 3) | + ((uint16_t)sbus_msg_bytes[14] << 5)) & + 0x07FF; + sbus_msg.channels[10] = (((uint16_t)sbus_msg_bytes[14] >> 6) | + ((uint16_t)sbus_msg_bytes[15] << 2) | + ((uint16_t)sbus_msg_bytes[16] << 10)) & + 0x07FF; + sbus_msg.channels[11] = (((uint16_t)sbus_msg_bytes[16] >> 1) | + ((uint16_t)sbus_msg_bytes[17] << 7)) & + 0x07FF; + sbus_msg.channels[12] = (((uint16_t)sbus_msg_bytes[17] >> 4) | + ((uint16_t)sbus_msg_bytes[18] << 4)) & + 0x07FF; + sbus_msg.channels[13] = (((uint16_t)sbus_msg_bytes[18] >> 7) | + ((uint16_t)sbus_msg_bytes[19] << 1) | + ((uint16_t)sbus_msg_bytes[20] << 9)) & + 0x07FF; + sbus_msg.channels[14] = (((uint16_t)sbus_msg_bytes[20] >> 2) | + ((uint16_t)sbus_msg_bytes[21] << 6)) & + 0x07FF; + sbus_msg.channels[15] = (((uint16_t)sbus_msg_bytes[21] >> 5) | + ((uint16_t)sbus_msg_bytes[22] << 3)) & + 0x07FF; + + // Decode flags from byte 23 (see comments in sendSBusMessage function) + // By default, flags are set to false in SBusMsg constructor, so we only need + // to set them true if necessary + if (sbus_msg_bytes[23] & (1 << 0)) { + sbus_msg.digital_channel_1 = true; + } + + if (sbus_msg_bytes[23] & (1 << 1)) { + sbus_msg.digital_channel_2 = true; + } + + if (sbus_msg_bytes[23] & (1 << 2)) { + sbus_msg.frame_lost = true; + } + + if (sbus_msg_bytes[23] & (1 << 3)) { + sbus_msg.failsafe = true; + } + + return sbus_msg; +} + +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_component.cpp b/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_component.cpp new file mode 100644 index 00000000..6fcc3437 --- /dev/null +++ b/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_component.cpp @@ -0,0 +1,156 @@ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +using namespace std::chrono_literals; + +class SO3CmdToSBUS : public rclcpp::Node +{ +public: + explicit SO3CmdToSBUS(const rclcpp::NodeOptions &options); + +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 check_so3_cmd_timeout(); + void motors_on(); + void motors_off(); + + // Controller state + bool odom_set_, imu_set_, so3_cmd_set_; + Eigen::Quaterniond odom_q_, imu_q_; + + // Subscribers + rclcpp::Subscription::SharedPtr so3_cmd_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + + // Motor status + int motor_status_; + + // Timeout handling[18][21] + double so3_cmd_timeout_; + rclcpp::Time last_so3_cmd_time_; + kr_mav_msgs::msg::SO3Command last_so3_cmd_; + + // SBUS bridge + std::shared_ptr sbus_bridge_; + rclcpp::TimerBase::SharedPtr init_timer_; +}; + +SO3CmdToSBUS::SO3CmdToSBUS(const rclcpp::NodeOptions &options) + : Node("so3cmd_to_sbus", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + odom_set_(false), + imu_set_(false), + so3_cmd_set_(false), + motor_status_(0) +{ + this->declare_parameter("so3_cmd_timeout", 0.25); + so3_cmd_timeout_ = this->get_parameter("so3_cmd_timeout").as_double(); + + // Create subscribers + so3_cmd_sub_ = this->create_subscription( + "so3_cmd", 10, + std::bind(&SO3CmdToSBUS::so3_cmd_callback, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + "odom", 10, + std::bind(&SO3CmdToSBUS::odom_callback, this, std::placeholders::_1)); + + imu_sub_ = this->create_subscription( + "imu", 10, + std::bind(&SO3CmdToSBUS::imu_callback, this, std::placeholders::_1)); + + init_timer_ = this->create_wall_timer( + std::chrono::milliseconds(0), + [this]() { + this->init_timer_->cancel(); + sbus_bridge_ = std::make_shared(shared_from_this()); + } + ); + + RCLCPP_INFO(this->get_logger(), "SO3CmdToSbus component initialized"); +} + +void SO3CmdToSBUS::so3_cmd_callback(const kr_mav_msgs::msg::SO3Command::SharedPtr msg) +{ + if (!so3_cmd_set_) + so3_cmd_set_ = true; + + // Switch on motors + if (msg->aux.enable_motors && !sbus_bridge_->isBridgeArmed() && !motor_status_) + { + motors_on(); + } + else if (!msg->aux.enable_motors) + { + motors_off(); + } + + sbus_bridge_->controlCommandCallback(msg, odom_q_); + + // Save last so3_cmd + last_so3_cmd_ = *msg; + last_so3_cmd_time_ = this->now(); +} + +void SO3CmdToSBUS::odom_callback(const nav_msgs::msg::Odometry::SharedPtr odom) +{ + if (!odom_set_) + odom_set_ = true; + + odom_q_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, + odom->pose.pose.orientation.x, + odom->pose.pose.orientation.y, + odom->pose.pose.orientation.z); + + check_so3_cmd_timeout(); +} + +void SO3CmdToSBUS::imu_callback(const sensor_msgs::msg::Imu::SharedPtr pose) +{ + if (!imu_set_) + imu_set_ = true; + + imu_q_ = Eigen::Quaterniond(pose->orientation.w, pose->orientation.x, + pose->orientation.y, pose->orientation.z); + + check_so3_cmd_timeout(); +} + +void SO3CmdToSBUS::check_so3_cmd_timeout() +{ + if (so3_cmd_set_ && ((this->now() - last_so3_cmd_time_).seconds() >= so3_cmd_timeout_)) + { + RCLCPP_DEBUG(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 SO3CmdToSBUS::motors_on() +{ + RCLCPP_INFO(this->get_logger(), "Motors on"); + sbus_bridge_->armBridge(); + motor_status_ = 1; +} + +void SO3CmdToSBUS::motors_off() +{ + sbus_bridge_->disarmBridge(); + motor_status_ = 0; +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(SO3CmdToSBUS) diff --git a/interfaces/kr_sbus_interface/src/thrust_mapping.cpp b/interfaces/kr_sbus_interface/src/thrust_mapping.cpp new file mode 100644 index 00000000..e0b8a2a8 --- /dev/null +++ b/interfaces/kr_sbus_interface/src/thrust_mapping.cpp @@ -0,0 +1,85 @@ +#include "kr_sbus_interface/thrust_mapping.h" + +#include + +namespace thrust_mapping +{ + +CollectiveThrustMapping::CollectiveThrustMapping() + : thrust_map_a_(0.0), + thrust_map_b_(0.0), + thrust_map_c_(0.0), + perform_thrust_voltage_compensation_(false), + thrust_ratio_voltage_map_a_(0.0), + thrust_ratio_voltage_map_b_(0.0), + n_lipo_cells_(0) +{ +} + +CollectiveThrustMapping::CollectiveThrustMapping(const double thrust_map_a, const double thrust_map_b, + const double thrust_map_c, + const bool perform_thrust_voltage_compensation, + const double thrust_ratio_voltage_map_a, + const double thrust_ratio_voltage_map_b, const int n_lipo_cells) + : thrust_map_a_(thrust_map_a), + thrust_map_b_(thrust_map_b), + thrust_map_c_(thrust_map_c), + perform_thrust_voltage_compensation_(perform_thrust_voltage_compensation), + thrust_ratio_voltage_map_a_(thrust_ratio_voltage_map_a), + thrust_ratio_voltage_map_b_(thrust_ratio_voltage_map_b), + n_lipo_cells_(n_lipo_cells) +{ +} + +CollectiveThrustMapping::~CollectiveThrustMapping() {} + +uint16_t CollectiveThrustMapping::inverseThrustMapping(const double thrust, const double battery_voltage) const +{ + double thrust_applied = thrust; + if(perform_thrust_voltage_compensation_) + { + if(battery_voltage >= n_lipo_cells_ * kMinBatteryCompensationVoltagePerCell_ && + battery_voltage <= n_lipo_cells_ * kMaxBatteryCompensationVoltagePerCell_) + { + const double thrust_cmd_voltage_ratio = + thrust_ratio_voltage_map_a_ * battery_voltage + thrust_ratio_voltage_map_b_; + thrust_applied *= thrust_cmd_voltage_ratio; + } + else + { + ROS_WARN_THROTTLE(1.0, "[%s] Battery voltage out of range for compensation", ros::this_node::getName().c_str()); + } + } + + // Citardauq Formula: Gives a numerically stable solution of the quadratic equation for thrust_map_a ~ 0, which is not + // the case for the standard formula. + const uint16_t cmd = + 2.0 * (thrust_map_c_ - thrust_applied) / + (-thrust_map_b_ - sqrt(thrust_map_b_ * thrust_map_b_ - 4.0 * thrust_map_a_ * (thrust_map_c_ - thrust_applied))); + + return cmd; +} + +bool CollectiveThrustMapping::loadParameters() +{ + ros::NodeHandle pnh("~"); + + if(!pnh.getParam("thrust_map_a", thrust_map_a_)) + return false; + if(!pnh.getParam("thrust_map_b", thrust_map_b_)) + return false; + if(!pnh.getParam("thrust_map_c", thrust_map_c_)) + return false; + if(!pnh.getParam("perform_thrust_voltage_compensation", perform_thrust_voltage_compensation_)) + return false; + if(!pnh.getParam("thrust_ratio_voltage_map_a", thrust_ratio_voltage_map_a_)) + return false; + if(!pnh.getParam("thrust_ratio_voltage_map_b", thrust_ratio_voltage_map_b_)) + return false; + if(!pnh.getParam("n_lipo_cells", n_lipo_cells_)) + return false; + + return true; +} + +} // namespace thrust_mapping From abdf1e2efd43b4d4d1bceb096f6fa9b517cd2f0d Mon Sep 17 00:00:00 2001 From: Dexter Date: Tue, 8 Jul 2025 19:37:41 +0000 Subject: [PATCH 39/64] Fixed logger to sbus_bridge --- interfaces/kr_sbus_interface/src/sbus_bridge.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/interfaces/kr_sbus_interface/src/sbus_bridge.cpp b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp index f8ad9648..67e63b34 100644 --- a/interfaces/kr_sbus_interface/src/sbus_bridge.cpp +++ b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp @@ -4,15 +4,13 @@ #include -// #include #include -#include - namespace sbus_bridge { SBusBridge::SBusBridge(const rclcpp::Node::SharedPtr& node) : node_(node), + logger_(node->get_logger()), stop_watchdog_thread_(false), time_last_rc_msg_received_(), time_last_sbus_msg_sent_(node_->now()), @@ -654,6 +652,7 @@ bool SBusBridge::loadParameters() !node_->get_parameter("rpm_vs_throttle_quadratic_coeff_b_", rpm_vs_throttle_quadratic_coeff_b_) || !node_->get_parameter("rpm_vs_throttle_quadratic_coeff_c_", rpm_vs_throttle_quadratic_coeff_c_) || !node_->get_parameter("use_body_rates", use_body_rates_)) { + RCLCPP_ERROR(logger_, "Failed to load one or more parameters."); return false; } return true; From 2def3a06479937ac90ac4724958a52dfd389dfc6 Mon Sep 17 00:00:00 2001 From: Dexter Date: Wed, 9 Jul 2025 16:17:00 -0400 Subject: [PATCH 40/64] Fixed ros2 clock --- .../kr_sbus_interface/config/neurofly.yaml | 2 +- .../kr_sbus_interface/sbus_serial_port.h | 7 +++-- .../launch/sbus_interface.py | 2 +- .../kr_sbus_interface/src/sbus_bridge.cpp | 14 +++++---- .../src/sbus_serial_port.cpp | 29 +++++++++++++------ 5 files changed, 35 insertions(+), 19 deletions(-) diff --git a/interfaces/kr_sbus_interface/config/neurofly.yaml b/interfaces/kr_sbus_interface/config/neurofly.yaml index b7017a4e..4c40a9a5 100644 --- a/interfaces/kr_sbus_interface/config/neurofly.yaml +++ b/interfaces/kr_sbus_interface/config/neurofly.yaml @@ -1,4 +1,4 @@ -port_name: /dev/ttyTHS0 +port_name: /dev/ttyTHS1 enable_receiving_sbus_messages: true control_command_timeout: 0.5 # [s] (Must be larger than 'state_estimate_timeout' # set in the 'flight_controller'!) diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h index 4e8d3494..25994207 100644 --- a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h +++ b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h @@ -11,12 +11,14 @@ class SBusSerialPort { public: SBusSerialPort(); SBusSerialPort(const std::string& port, - const bool start_receiver_thread); + const bool start_receiver_thread, + const rclcpp::Clock::SharedPtr& clock = nullptr); virtual ~SBusSerialPort(); protected: bool setUpSBusSerialPort(const std::string& port, - const bool start_receiver_thread); + const bool start_receiver_thread, + const rclcpp::Clock::SharedPtr& clock = nullptr); bool connectSerialPort(const std::string& port); void disconnectSerialPort(); @@ -44,6 +46,7 @@ class SBusSerialPort { int serial_port_fd_; rclcpp::Logger logger_ = rclcpp::get_logger("sbus_serial_port"); + rclcpp::Clock::SharedPtr clock_; }; } // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/launch/sbus_interface.py b/interfaces/kr_sbus_interface/launch/sbus_interface.py index 6cb8db49..2d068ced 100644 --- a/interfaces/kr_sbus_interface/launch/sbus_interface.py +++ b/interfaces/kr_sbus_interface/launch/sbus_interface.py @@ -35,7 +35,7 @@ def generate_launch_description(): ('odom', odom_topic), ('so3_cmd', so3_cmd_topic), ], - parameters=[config_file, {'port_name': '/dev/ttyTHS0'}], + parameters=[config_file], ), ], output='screen', diff --git a/interfaces/kr_sbus_interface/src/sbus_bridge.cpp b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp index 67e63b34..aca6a843 100644 --- a/interfaces/kr_sbus_interface/src/sbus_bridge.cpp +++ b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp @@ -34,7 +34,7 @@ namespace sbus_bridge { // Start serial port with receiver thread if receiving sbus messages is // enabled - if (!setUpSBusSerialPort(port_name_, enable_receiving_sbus_messages_)) { + if (!setUpSBusSerialPort(port_name_, enable_receiving_sbus_messages_, node->get_clock())) { rclcpp::shutdown(); return; } @@ -49,6 +49,8 @@ namespace sbus_bridge { rclcpp::shutdown(); return; } + + RCLCPP_INFO(logger_, "SBusBridge initialized"); } SBusBridge::~SBusBridge() { @@ -157,7 +159,7 @@ void SBusBridge::handleReceivedSbusMessage(const SBusMsg &received_sbus_msg) { std::lock_guard main_lock(main_mutex_); - time_last_rc_msg_received_ = rclcpp::Clock().now(); + time_last_rc_msg_received_ = node_->now(); if (received_sbus_msg.isKillSwitch()) { @@ -255,7 +257,7 @@ void SBusBridge::controlCommandCallback( } // may need more logic @TODO - time_last_active_control_command_received_ = rclcpp::Clock().now(); + time_last_active_control_command_received_ = node_->now(); if (!bridge_armed_ || bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) { @@ -375,7 +377,7 @@ void SBusBridge::sendSBusMessageToSerialPort(const SBusMsg& sbus_msg) break; } - if ((rclcpp::Clock().now() - time_last_sbus_msg_sent_).seconds() <= 0.006) { + if ((node_->now() - time_last_sbus_msg_sent_).seconds() <= 0.006) { // An SBUS message takes 3ms to be transmitted by the serial port so let's // not stress it too much. This should only happen in case of switching // between control commands and rc commands @@ -389,9 +391,9 @@ void SBusBridge::sendSBusMessageToSerialPort(const SBusMsg& sbus_msg) return; } - sbus_message_to_send.timestamp = rclcpp::Clock().now(); + sbus_message_to_send.timestamp = node_->now(); transmitSerialSBusMessage(sbus_message_to_send); - time_last_sbus_msg_sent_ = rclcpp::Clock().now(); + time_last_sbus_msg_sent_ = node_->now(); } static std::pair solve_quadratic(double a, double b, double c) diff --git a/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp b/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp index a8a573cd..20cc7ca3 100644 --- a/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp +++ b/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp @@ -14,25 +14,29 @@ namespace sbus_bridge { SBusSerialPort::SBusSerialPort() - : receiver_thread_should_exit_(false), - serial_port_fd_(-1) { - // Optional: Initialize with default values + : receiver_thread_(), + receiver_thread_should_exit_(false), + serial_port_fd_(-1), + clock_(nullptr) { logger_ = rclcpp::get_logger("sbus_serial_port"); } SBusSerialPort::SBusSerialPort(const std::string& port, - const bool start_receiver_thread) + const bool start_receiver_thread, + const rclcpp::Clock::SharedPtr& clock) : receiver_thread_(), receiver_thread_should_exit_(false), - serial_port_fd_(-1) { - setUpSBusSerialPort(port, start_receiver_thread); + serial_port_fd_(-1), + clock_(clock) { logger_ = rclcpp::get_logger("sbus_serial_port"); } SBusSerialPort::~SBusSerialPort() { disconnectSerialPort(); } bool SBusSerialPort::setUpSBusSerialPort(const std::string& port, - const bool start_receiver_thread) { + const bool start_receiver_thread, + const rclcpp::Clock::SharedPtr& clock) { + clock_ = clock; if (!connectSerialPort(port)) { return false; } @@ -69,7 +73,10 @@ bool SBusSerialPort::connectSerialPort(const std::string& port) { void SBusSerialPort::disconnectSerialPort() { stopReceiverThread(); - close(serial_port_fd_); + if (serial_port_fd_ != -1) { + close(serial_port_fd_); + serial_port_fd_ = -1; + } } bool SBusSerialPort::startReceiverThread() { @@ -317,7 +324,11 @@ SBusMsg SBusSerialPort::parseSbusMessage( uint8_t sbus_msg_bytes[kSbusFrameLength_]) const { SBusMsg sbus_msg; - sbus_msg.timestamp = rclcpp::Clock().now(); + if (clock_) { + sbus_msg.timestamp = clock_->now(); + } else { + sbus_msg.timestamp = rclcpp::Clock().now(); + } // Decode the 16 regular channels sbus_msg.channels[0] = From f57367fd805c9dc531da2c068f73029df5ec5afb Mon Sep 17 00:00:00 2001 From: Dexter Date: Mon, 4 Aug 2025 10:35:45 -0400 Subject: [PATCH 41/64] Updated sbus_interface launch file for VIO flight --- .../kr_sbus_interface/cmake/FindSnav.cmake | 21 -- .../kr_sbus_interface/config/gains.yaml | 27 +-- .../launch/control.launch.py | 213 ++++++++++++++++++ ..._interface.py => sbus_interface.launch.py} | 0 .../kr_sbus_interface/msg/SbusRosMessage.msg | 14 -- .../kr_sbus_interface/src/sbus_bridge.cpp | 12 +- .../src/so3cmd_to_sbus_component.cpp | 8 +- 7 files changed, 237 insertions(+), 58 deletions(-) delete mode 100644 interfaces/kr_sbus_interface/cmake/FindSnav.cmake create mode 100644 interfaces/kr_sbus_interface/launch/control.launch.py rename interfaces/kr_sbus_interface/launch/{sbus_interface.py => sbus_interface.launch.py} (100%) delete mode 100644 interfaces/kr_sbus_interface/msg/SbusRosMessage.msg diff --git a/interfaces/kr_sbus_interface/cmake/FindSnav.cmake b/interfaces/kr_sbus_interface/cmake/FindSnav.cmake deleted file mode 100644 index 07f3cd69..00000000 --- a/interfaces/kr_sbus_interface/cmake/FindSnav.cmake +++ /dev/null @@ -1,21 +0,0 @@ -FIND_PATH(Snav_INCLUDE_DIR - NAMES - snav/snapdragon_navigator.h - PATHS - /usr/include - NO_DEFAULT_PATH -) - -FIND_LIBRARY(Snav_LIBRARY - NAMES - snav_arm - PATHS - /usr/lib - NO_DEFAULT_PATH -) - -IF(Snav_INCLUDE_DIR AND Snav_LIBRARY) - SET(Snav_FOUND TRUE) -ELSE(Snav_INCLUDE_DIR AND Snav_LIBRARY) - SET(Snav_FOUND FALSE) -ENDIF(Snav_INCLUDE_DIR AND Snav_LIBRARY) diff --git a/interfaces/kr_sbus_interface/config/gains.yaml b/interfaces/kr_sbus_interface/config/gains.yaml index 0c57f8a8..7d5eb78e 100644 --- a/interfaces/kr_sbus_interface/config/gains.yaml +++ b/interfaces/kr_sbus_interface/config/gains.yaml @@ -1,15 +1,16 @@ -gains: - pos: {x: 3.4, y: 3.4, z: 5.4} - vel: {x: 2.8, y: 2.8, z: 3.0} - ki: {x: 0.00, y: 0.00, z: 0.00, yaw: 0.01} - kib: {x: 0.00, y: 0.00, z: 0.00} - rot: {x: 1.5, y: 1.5, z: 1.0} - ang: {x: 0.13, y: 0.13, z: 0.1} +neurofly1/so3_controller: + gains: + pos: { x: 6.0, y: 6.0, z: 8.0 } + vel: { x: 3.0, y: 3.0, z: 3.5 } + ki: { x: 5.00, y: 5.00, z: 5.00 } + kib: { x: 0.00, y: 0.00, z: 0.00 } + rot: { x: 1.2, y: 1.2, z: 1.0 } + ang: { x: 0.13, y: 0.13, z: 0.1} -corrections: - kf: 0.0e-08 - r: 0.0 - p: 0.0 + corrections: + kf: 0.0e-08 + r: 0.0 + p: 0.0 -max_pos_int: 0.5 -max_pos_int_b: 0.5 \ No newline at end of file + max_pos_int: 0.3 + max_pos_int_b: 0.3 \ No newline at end of file diff --git a/interfaces/kr_sbus_interface/launch/control.launch.py b/interfaces/kr_sbus_interface/launch/control.launch.py new file mode 100644 index 00000000..577e7a83 --- /dev/null +++ b/interfaces/kr_sbus_interface/launch/control.launch.py @@ -0,0 +1,213 @@ +import os +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, GroupAction, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node, ComposableNodeContainer, PushRosNamespace, LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch.conditions import LaunchConfigurationEquals, IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression, Command, TextSubstitution +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + # KR interface paths & configurations + config_file = os.path.join( + get_package_share_directory('kr_sbus_interface'), + 'config', + 'neurofly.yaml' + ) + + trackers_manager_config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml' + so3_config_file = os.path.join( + get_package_share_directory('kr_sbus_interface'), + 'config', + 'gains.yaml' + ) + + # URDF/xacro file to be loaded by the Robot State Publisher node + default_xacro_path = os.path.join( + get_package_share_directory('zed_wrapper'), + 'urdf', + 'zed_descr.urdf.xacro' + ) + + # Define launch arguments + vicon_args = [ + # Adding mocap vicon specific parameters + DeclareLaunchArgument('mocap', default_value='false'), + DeclareLaunchArgument('mocap_server', default_value='mocap.perch'), + DeclareLaunchArgument('mocap_frame_rate', default_value='100'), + DeclareLaunchArgument('mocap_max_accel', default_value='10.0'), + ] + + # KR interface arguments + kr_args = [ + DeclareLaunchArgument('robot', default_value='neurofly1'), + DeclareLaunchArgument('odom', default_value='odom'), + DeclareLaunchArgument('so3_cmd', default_value='so3_cmd'), + DeclareLaunchArgument('mass', default_value='.680'), + ] + + # ZED camera arguments + zed_args = [ + DeclareLaunchArgument('zed_enable', default_value='true'), + DeclareLaunchArgument('camera_name', default_value='zed'), + DeclareLaunchArgument('camera_model', default_value='zedm'), + DeclareLaunchArgument('publish_urdf', default_value='true'), + DeclareLaunchArgument('publish_tf', default_value='true'), + DeclareLaunchArgument('publish_map_tf', default_value='true'), + DeclareLaunchArgument('publish_imu_tf', default_value='true'), + DeclareLaunchArgument('xacro_path', default_value=TextSubstitution(text=default_xacro_path)), + DeclareLaunchArgument('custom_baseline', default_value='0.0'), + DeclareLaunchArgument('enable_gnss', default_value='false'), + DeclareLaunchArgument('publish_svo_clock', default_value='false'), + ] + + # Initialize launch description with all arguments + ld = LaunchDescription(vicon_args + kr_args + zed_args) + + # Create a main component container for all composable nodes + main_container = ComposableNodeContainer( + name="control_container", + namespace="", # Empty namespace for container, individual nodes will have their own namespaces + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="kr_sbus_interface", + plugin="SO3CmdToSBUS", + name="so3cmd_to_sbus", + namespace=LaunchConfiguration('robot'), + parameters=[config_file], + remappings=[ + ('so3_cmd', LaunchConfiguration('so3_cmd')), + ('odom', LaunchConfiguration('odom')), + ('imu', 'zed/zed_node/imu/data'), + ] + ), + ComposableNode( + package="kr_trackers_manager", + plugin="TrackersManager", + name="trackers_manager", + namespace=LaunchConfiguration('robot'), + parameters=[trackers_manager_config_file], + ), + ComposableNode( + package="kr_trackers_manager", + plugin="TrackersManagerLifecycleManager", + name="lifecycle_manager", + namespace=LaunchConfiguration('robot'), + parameters=[ + {'node_name': "trackers_manager"} + ] + ), + ComposableNode( + package="kr_mav_controllers", + plugin="SO3ControlComponent", + name="so3_controller", + namespace=LaunchConfiguration('robot'), + parameters=[so3_config_file], + ), + ComposableNode( + condition=IfCondition(LaunchConfiguration('zed_enable')), + package="zed_components", + plugin="stereolabs::ZedCamera", + name="zed_node", + namespace="zed", + parameters=[ + [FindPackageShare('zed_wrapper').find('zed_wrapper'), '/config/', LaunchConfiguration('camera_model'), '.yaml'], + [FindPackageShare('zed_wrapper').find('zed_wrapper'), '/config/common_stereo.yaml'], + # Finally apply launch-specific overrides + { + 'general.camera_name': LaunchConfiguration('camera_name'), + 'general.camera_model': LaunchConfiguration('camera_model'), + 'pos_tracking.publish_tf': LaunchConfiguration('publish_tf'), + 'pos_tracking.publish_map_tf': LaunchConfiguration('publish_map_tf'), + 'sensors.publish_imu_tf': LaunchConfiguration('publish_imu_tf', default='true'), + } + ], + extra_arguments=[{'use_intra_process_comms': True}] + ), + ComposableNode( + package="quadrotor_ukf_ros2", + plugin="QuadrotorUKFNode", + name="quadrotor_ukf_ros2", + namespace=LaunchConfiguration('robot'), + parameters=[{ + 'odom': LaunchConfiguration('odom'), + 'imu_frame_id': 'zed_imu_link', + 'imu_rotated_frame_id': 'zed_camera_link', + 'base_link': 'zed_camera_link' + }], + remappings=[ + ('odom', '/zed/zed_node/odom'), + ('imu', '/zed/zed_node/imu/data'), + ], + ), + ], + output='screen', + ) + + ld.add_action(main_container) + + # robot state publisher to publish URDF and static transforms for zed + ld.add_action( + Node( + condition=IfCondition(LaunchConfiguration('zed_enable')), + package='robot_state_publisher', + executable='robot_state_publisher', + name=[LaunchConfiguration('camera_name'), '_state_publisher'], + namespace=LaunchConfiguration('robot'), + output='screen', + parameters=[{ + 'use_sim_time': LaunchConfiguration('publish_svo_clock'), + 'robot_description': Command([ + 'xacro', ' ', LaunchConfiguration('xacro_path'), + ' camera_name:=', LaunchConfiguration('camera_name'), + ' camera_model:=', LaunchConfiguration('camera_model'), + ' custom_baseline:=', LaunchConfiguration('custom_baseline') + ]) + }] + ) + ) + + ld.add_action( + Node( + condition=IfCondition(LaunchConfiguration('mocap')), + package='mocap_vicon', + executable='mocap_vicon_node', + name='vicon', + output='screen', + parameters=[ + {'server_address': LaunchConfiguration('mocap_server')}, + {'frame_rate': LaunchConfiguration('mocap_frame_rate')}, + {'max_accel': LaunchConfiguration('mocap_max_accel')}, + {'publish_tf': False}, + {'publish_pts': False}, + {'fixed_frame_id': 'mocap'}, + # Set to [''] to take in ALL models from Vicon + {'model_list': ['neurofly1']}, + ], + remappings=[ + # Uncomment and modify the remapping if needed + ('/neurofly1/odom', '/odom'), + ] + ) + ) + + ld.add_action( + Node( + package="kr_mav_manager", + executable="mav_services", + namespace=LaunchConfiguration('robot'), + name="mav_services", + output='screen', + parameters = [ + {'mass': LaunchConfiguration('mass')}, + ], + ) + ) + + return ld diff --git a/interfaces/kr_sbus_interface/launch/sbus_interface.py b/interfaces/kr_sbus_interface/launch/sbus_interface.launch.py similarity index 100% rename from interfaces/kr_sbus_interface/launch/sbus_interface.py rename to interfaces/kr_sbus_interface/launch/sbus_interface.launch.py diff --git a/interfaces/kr_sbus_interface/msg/SbusRosMessage.msg b/interfaces/kr_sbus_interface/msg/SbusRosMessage.msg deleted file mode 100644 index 3802e30f..00000000 --- a/interfaces/kr_sbus_interface/msg/SbusRosMessage.msg +++ /dev/null @@ -1,14 +0,0 @@ -Header header - -# Regular 16 sbus channels with 11 bit value range each -uint16[16] channels - -# Digital channels -bool digital_channel_1 -bool digital_channel_2 - -# Frame lost flag -bool frame_lost - -# Failsafe flag -bool failsafe diff --git a/interfaces/kr_sbus_interface/src/sbus_bridge.cpp b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp index aca6a843..55230ffd 100644 --- a/interfaces/kr_sbus_interface/src/sbus_bridge.cpp +++ b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp @@ -487,7 +487,7 @@ SBusMsg SBusBridge::generateSBusMessageFromSO3Command(const kr_mav_msgs::msg::SO pitch_cmd = round((pitch_rate + max_pitch_rate_) / (2 * max_pitch_rate_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); RCLCPP_INFO_THROTTLE( - logger_, *node_->get_clock(), 1.0, + logger_, *node_->get_clock(), 1000, "AUTONOMOUS MODE: roll_rate: %f pitch_rate: %f roll_cmd: %d pitch_cmd: %d", roll_rate, pitch_rate, roll_cmd, pitch_cmd); } @@ -510,7 +510,7 @@ SBusMsg SBusBridge::generateSBusMessageFromSO3Command(const kr_mav_msgs::msg::SO pitch_cmd = round((pitch_deg + max_pitch_angle_) / (2 * max_pitch_angle_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); RCLCPP_INFO_THROTTLE( - logger_, *node_->get_clock(), 1.0, + logger_, *node_->get_clock(), 1000, "AUTONOMOUS MODE: roll_deg: %f pitch_deg: %f roll_cmd: %d pitch_cmd: %d", roll_deg, pitch_deg, roll_cmd, pitch_cmd); } @@ -521,10 +521,10 @@ SBusMsg SBusBridge::generateSBusMessageFromSO3Command(const kr_mav_msgs::msg::SO yaw_rate = std::max(-400.0, std::min(400.0, yaw_rate)); // clip max yaw rate yaw_cmd = round((yaw_rate + max_yaw_rate_) / (2 * max_yaw_rate_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); - RCLCPP_INFO_THROTTLE( - logger_, *node_->get_clock(), 1.0, - "AUTONOMOUS MODE: yaw_rate: %f yaw_cmd: %d", - yaw_rate, yaw_cmd); +// RCLCPP_INFO_THROTTLE( +// logger_, *node_->get_clock(), 1.0, +// "AUTONOMOUS MODE: yaw_rate: %f yaw_cmd: %d", +// yaw_rate, yaw_cmd); sbus_msg.setRollCommand(roll_cmd); sbus_msg.setPitchCommand(pitch_cmd); diff --git a/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_component.cpp b/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_component.cpp index 6fcc3437..62c12cd4 100644 --- a/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_component.cpp +++ b/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_component.cpp @@ -32,7 +32,7 @@ class SO3CmdToSBUS : public rclcpp::Node // Subscribers rclcpp::Subscription::SharedPtr so3_cmd_sub_; rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr imu_sub_; +// rclcpp::Subscription::SharedPtr imu_sub_; // Motor status int motor_status_; @@ -66,9 +66,9 @@ SO3CmdToSBUS::SO3CmdToSBUS(const rclcpp::NodeOptions &options) "odom", 10, std::bind(&SO3CmdToSBUS::odom_callback, this, std::placeholders::_1)); - imu_sub_ = this->create_subscription( - "imu", 10, - std::bind(&SO3CmdToSBUS::imu_callback, this, std::placeholders::_1)); +// imu_sub_ = this->create_subscription( +// "imu", 10, +// std::bind(&SO3CmdToSBUS::imu_callback, this, std::placeholders::_1)); init_timer_ = this->create_wall_timer( std::chrono::milliseconds(0), From c25090a4346b44f2f1081f106947413ea6de3eb7 Mon Sep 17 00:00:00 2001 From: Dexter Date: Fri, 12 Sep 2025 17:15:54 -0400 Subject: [PATCH 42/64] First cut betaflight_interface --- .../kr_betaflight_interface/CMakeLists.txt | 68 ++ .../kr_betaflight_interface/config/gains.yaml | 15 + .../config/mav_manager_params.yaml | 6 + .../config/neurofly.yaml | 32 + .../config/tracker_params.yaml | 25 + .../config/trackers.yaml | 11 + .../crsf/crsf_bridge.h | 152 +++++ .../crsf/crsf_channel_mapping.h | 18 + .../kr_betaflight_interface/crsf/crsf_msg.h | 63 ++ .../crsf/crsf_serial_port.h | 48 ++ .../protocol_bridge_base.h | 16 + .../protocol_msg_base.h | 9 + .../sbus/sbus_bridge.h | 152 +++++ .../sbus/sbus_channel_mapping.h | 19 + .../kr_betaflight_interface/sbus/sbus_msg.h | 64 ++ .../sbus/sbus_serial_port.h | 49 ++ .../launch/betaflight_interface.launch.py | 57 ++ .../launch/control.launch.py | 213 ++++++ .../kr_betaflight_interface/package.xml | 23 + .../scripts/mavlink_telemetry.py | 35 + .../src/crsf/crsf_bridge.cpp | 612 +++++++++++++++++ .../src/crsf/crsf_msg.cpp | 103 +++ .../src/crsf/crsf_serial_port.cpp | 326 +++++++++ .../src/sbus/sbus_bridge.cpp | 636 ++++++++++++++++++ .../src/sbus/sbus_msg.cpp | 144 ++++ .../src/sbus/sbus_serial_port.cpp | 405 +++++++++++ .../src/so3cmd_to_betaflight_component.cpp | 148 ++++ 27 files changed, 3449 insertions(+) create mode 100644 interfaces/kr_betaflight_interface/CMakeLists.txt create mode 100644 interfaces/kr_betaflight_interface/config/gains.yaml create mode 100644 interfaces/kr_betaflight_interface/config/mav_manager_params.yaml create mode 100644 interfaces/kr_betaflight_interface/config/neurofly.yaml create mode 100644 interfaces/kr_betaflight_interface/config/tracker_params.yaml create mode 100644 interfaces/kr_betaflight_interface/config/trackers.yaml create mode 100644 interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_bridge.h create mode 100644 interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_channel_mapping.h create mode 100644 interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_msg.h create mode 100644 interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_serial_port.h create mode 100644 interfaces/kr_betaflight_interface/include/kr_betaflight_interface/protocol_bridge_base.h create mode 100644 interfaces/kr_betaflight_interface/include/kr_betaflight_interface/protocol_msg_base.h create mode 100644 interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_bridge.h create mode 100644 interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_channel_mapping.h create mode 100644 interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_msg.h create mode 100644 interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_serial_port.h create mode 100644 interfaces/kr_betaflight_interface/launch/betaflight_interface.launch.py create mode 100644 interfaces/kr_betaflight_interface/launch/control.launch.py create mode 100644 interfaces/kr_betaflight_interface/package.xml create mode 100644 interfaces/kr_betaflight_interface/scripts/mavlink_telemetry.py create mode 100644 interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp create mode 100644 interfaces/kr_betaflight_interface/src/crsf/crsf_msg.cpp create mode 100644 interfaces/kr_betaflight_interface/src/crsf/crsf_serial_port.cpp create mode 100644 interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp create mode 100644 interfaces/kr_betaflight_interface/src/sbus/sbus_msg.cpp create mode 100644 interfaces/kr_betaflight_interface/src/sbus/sbus_serial_port.cpp create mode 100644 interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp diff --git a/interfaces/kr_betaflight_interface/CMakeLists.txt b/interfaces/kr_betaflight_interface/CMakeLists.txt new file mode 100644 index 00000000..9ce9101c --- /dev/null +++ b/interfaces/kr_betaflight_interface/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.10) +project(kr_betaflight_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include_directories( + include + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(kr_mav_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/so3cmd_to_betaflight_component.cpp + src/crsf/crsf_bridge.cpp + src/crsf/crsf_msg.cpp + src/crsf/crsf_serial_port.cpp + src/sbus/sbus_bridge.cpp + src/sbus/sbus_msg.cpp + src/sbus/sbus_serial_port.cpp +) +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_components + kr_mav_msgs + nav_msgs + sensor_msgs + geometry_msgs + tf2) +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) +rclcpp_components_register_nodes(${PROJECT_NAME} "SO3CmdToBetaflight") + +# Install headers +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/interfaces/kr_betaflight_interface/config/gains.yaml b/interfaces/kr_betaflight_interface/config/gains.yaml new file mode 100644 index 00000000..f8868f19 --- /dev/null +++ b/interfaces/kr_betaflight_interface/config/gains.yaml @@ -0,0 +1,15 @@ +gains: + pos: {x: 3.4, y: 3.4, z: 5.4} + vel: {x: 2.8, y: 2.8, z: 3.0} + ki: {x: 0.00, y: 0.00, z: 0.00, yaw: 0.01} + kib: {x: 0.00, y: 0.00, z: 0.00} + rot: {x: 1.5, y: 1.5, z: 1.0} + ang: {x: 0.13, y: 0.13, z: 0.1} + +corrections: + kf: 0.0e-08 + r: 0.0 + p: 0.0 + +max_pos_int: 0.5 +max_pos_int_b: 0.5 \ No newline at end of file diff --git a/interfaces/kr_betaflight_interface/config/mav_manager_params.yaml b/interfaces/kr_betaflight_interface/config/mav_manager_params.yaml new file mode 100644 index 00000000..40685c46 --- /dev/null +++ b/interfaces/kr_betaflight_interface/config/mav_manager_params.yaml @@ -0,0 +1,6 @@ +server_wait_timeout: 0.5 +need_imu: false +need_output_data: true +use_attitude_safety_catch: true +max_attitude_angle: 0.43 +takeoff_height: 0.2 diff --git a/interfaces/kr_betaflight_interface/config/neurofly.yaml b/interfaces/kr_betaflight_interface/config/neurofly.yaml new file mode 100644 index 00000000..6044b65a --- /dev/null +++ b/interfaces/kr_betaflight_interface/config/neurofly.yaml @@ -0,0 +1,32 @@ +port_name: /dev/ttyTHS1 +enable_receiving_crsf_messages: true +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.66 # [kg] +disable_thrust_mapping: true +use_body_rates: false +# 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 +# when flying in angle mode +max_roll_rate: 1000.0 # [deg/s] +max_pitch_rate: 1000.0 # [deg/s] +max_yaw_rate: 1000.0 # [deg/s] +max_roll_angle: 50.0 # [deg] +max_pitch_angle: 50.0 # [deg] +alpha_vbat_filter: 0.01 +perform_thrust_voltage_compensation: false +thrust_ratio_voltage_map_a: -0.17044342 # [1/V] +thrust_ratio_voltage_map_b: 3.10495276 # [-] +n_lipo_cells: 3 # [-] diff --git a/interfaces/kr_betaflight_interface/config/tracker_params.yaml b/interfaces/kr_betaflight_interface/config/tracker_params.yaml new file mode 100644 index 00000000..64c0d4b9 --- /dev/null +++ b/interfaces/kr_betaflight_interface/config/tracker_params.yaml @@ -0,0 +1,25 @@ +# This should contain tracker parameters + +line_tracker_distance: + default_v_des: 1.0 + default_a_des: 0.5 + epsilon: 0.1 + +line_tracker_min_jerk: + default_v_des: 1.0 + default_a_des: 0.5 + default_yaw_v_des: 0.8 + default_yaw_a_des: 0.2 + +trajectory_tracker: + max_vel_des: 0.5 + max_acc_des: 0.3 + +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 new file mode 100644 index 00000000..9fc1551d --- /dev/null +++ b/interfaces/kr_betaflight_interface/config/trackers.yaml @@ -0,0 +1,11 @@ +trackers: + - kr_trackers/LineTrackerMinJerk + - kr_trackers/LineTrackerDistance +# - kr_trackers/LineTrackerTrapezoid +# - kr_trackers/LineTrackerYaw + - kr_trackers/VelocityTrackerAction + - kr_trackers/NullTracker + - kr_trackers/CircleTracker + - kr_trackers/TrajectoryTracker + - kr_trackers/SmoothVelTracker + - kr_trackers/LissajousTracker diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_bridge.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_bridge.h new file mode 100644 index 00000000..c2b6d18b --- /dev/null +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_bridge.h @@ -0,0 +1,152 @@ +#pragma once + +#include +#include "kr_betaflight_interface/crsf/crsf_msg.h" +#include "kr_betaflight_interface/protocol_bridge_base.h" +#include "kr_betaflight_interface/crsf/crsf_serial_port.h" +#include +#include +#include + +#include +#include +#include +#include + +namespace crsf_bridge +{ + +enum class BridgeState +{ + OFF, + ARMING, + AUTONOMOUS_FLIGHT, + RC_FLIGHT, + KILL +}; + +class CrsfBridge : public ProtocolBridgeBase +{ + public: + CrsfBridge(const rclcpp::Node::SharedPtr &node); + void controlCommandCallback(const kr_mav_msgs::msg::SO3Command::ConstPtr &msg, const Eigen::Quaterniond &odom_q) override; + void armBridge() override; + void disarmBridge() override; + bool isBridgeArmed() const override; + const ProtocolMsgBase* getLastProtocolMsg() const override { return last_msg_.get(); } + virtual ~CrsfBridge(); + + private: + void watchdogThread(); + + void handleReceivedCrsfMessage(const CrsfMsg &received_crsf_msg); + + CrsfMsg generateCrsfMessageFromSO3Command(const kr_mav_msgs::msg::SO3Command::ConstPtr &control_command, + const Eigen::Quaterniond &odom_q) const; + + void sendCrsfMessageToSerialPort(const CrsfMsg &crsf_msg); + + void setBridgeState(const BridgeState &desired_bridge_state); + + double thrust_model_kartik(double thrust) const; + + bool loadParameters(); + + // Serial port interface + std::unique_ptr serial_port_; + std::unique_ptr last_msg_; + // rclcpp Node + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + + // Mutex for: + // - bridge_state_ + // - control_mode_ + // - bridge_armed_ + // - time_last_active_control_command_received_ + // - time_last_rc_msg_received_ + // - arming_counter_ + // - time_last_crsf_msg_sent_ + // Also "setBridgeState" and "sendCrsfMessageToSerialPort" should only be + // called when "main_mutex_" is locked + mutable std::mutex main_mutex_; + mutable std::mutex arm_mutex_; + // Mutex for: + // - battery_voltage_ + // - time_last_battery_voltage_received_ + // Also "generateCrsfMessageFromControlCommand" should only be called when + // "battery_voltage_mutex_" is locked + mutable std::mutex battery_voltage_mutex_; + + // Subscribers + rclcpp::Subscription::SharedPtr control_command_sub_; + rclcpp::Subscription::SharedPtr battery_voltage_sub_; + // ROS 2: arm_bridge_sub_ removed unless you have a custom message/type for it + + // Timer + rclcpp::TimerBase::SharedPtr low_level_feedback_pub_timer_; + + // Watchdog + std::thread watchdog_thread_; + std::atomic_bool stop_watchdog_thread_; + rclcpp::Time time_last_rc_msg_received_; + rclcpp::Time time_last_crsf_msg_sent_; + rclcpp::Time time_last_battery_voltage_received_; + rclcpp::Time time_last_active_control_command_received_; + + BridgeState bridge_state_; + ControlMode control_mode_; + int arming_counter_; + double battery_voltage_; + + // Safety flags + bool bridge_armed_; + bool rc_was_disarmed_once_; + + std::atomic_bool destructor_invoked_; + + // Parameters + std::string port_name_; + bool enable_receiving_crsf_messages_; + + double control_command_timeout_; + double rc_timeout_; + + double mass_; + + bool disable_thrust_mapping_; + + double max_roll_rate_; + double max_pitch_rate_; + double max_yaw_rate_; + + double max_roll_angle_; + double max_pitch_angle_; + + double alpha_vbat_filter_; + bool perform_thrust_voltage_compensation_; + int n_lipo_cells_; + + // Constants + static constexpr double kLowLevelFeedbackPublishFrequency_ = 50.0; + + static constexpr int kSmoothingFailRepetitions_ = 5; + + static constexpr double kBatteryLowVoltagePerCell_ = 3.6; + static constexpr double kBatteryCriticalVoltagePerCell_ = 3.4; + static constexpr double kBatteryInvalidVoltagePerCell_ = 3.0; + static constexpr double kBatteryVoltageTimeout_ = 1.0; + + double thrust_vs_rpm_cof_a_; + double thrust_vs_rpm_cof_b_; + double thrust_vs_rpm_cof_c_; + double rpm_vs_throttle_linear_coeff_a_; + double rpm_vs_throttle_linear_coeff_b_; + double rpm_vs_throttle_quadratic_coeff_a_; + double rpm_vs_throttle_quadratic_coeff_b_; + double rpm_vs_throttle_quadratic_coeff_c_; + + bool use_body_rates_; +}; + +} // namespace crsf_bridge diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_channel_mapping.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_channel_mapping.h new file mode 100644 index 00000000..33d7beaa --- /dev/null +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_channel_mapping.h @@ -0,0 +1,18 @@ +#pragma once + +namespace crsf_bridge { + +namespace crsf_channel_mapping { + +static constexpr uint8_t kThrottle = 2; +static constexpr uint8_t kRoll = 0; +static constexpr uint8_t kPitch = 1; +static constexpr uint8_t kYaw = 3; +static constexpr uint8_t kControlMode = 5; +static constexpr uint8_t kArming = 4; +static constexpr uint8_t kKillSwitch = 7; +static constexpr uint8_t kGamepadMode = 6; + +} // namespace crsf_channel_mapping + +} // namespace crsf_bridge diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_msg.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_msg.h new file mode 100644 index 00000000..2b1d39a0 --- /dev/null +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_msg.h @@ -0,0 +1,63 @@ +#pragma once + +#include +#include +#include +#include +#include "kr_betaflight_interface/protocol_msg_base.h" + +namespace crsf_bridge { + + enum class ControlMode { NONE, ATTITUDE, BODY_RATES }; + enum class ArmState { DISARMED, ARMED }; + +#pragma pack(push) +#pragma pack(1) +struct CrsfMsg : public ProtocolMsgBase { + // Constants + static constexpr int kNChannels = 16; + static constexpr uint16_t kMinCmd = 172; // CRSF min value + static constexpr uint16_t kMeanCmd = 992; // CRSF mid value + static constexpr uint16_t kMaxCmd = 1811; // CRSF max value + + rclcpp::Time timestamp; + + // Normal channels + uint16_t channels[kNChannels]; + + // Digital channels + bool digital_channel_1; + bool digital_channel_2; + + // Flags + bool frame_lost; + bool failsafe; + + CrsfMsg(); + virtual ~CrsfMsg(); + + uint16_t getChannel(int idx) const override { return channels[idx]; } + void setChannel(int idx, uint16_t value) override { channels[idx] = value; } + + void limitAllChannelsFeasible(); + void limitCrsfChannelFeasible(const int channel_idx); + + // Setting CRSF command helpers + void setThrottleCommand(const uint16_t throttle_cmd); + void setRollCommand(const uint16_t roll_cmd); + void setPitchCommand(const uint16_t pitch_cmd); + void setYawCommand(const uint16_t yaw_cmd); + void setControlMode(const ControlMode& control_mode); + void setControlModeAttitude(); + void setControlModeBodyRates(); + void setArmState(const ArmState& arm_state); + void setArmStateArmed(); + void setArmStateDisarmed(); + + // CRSF message check helpers + bool isArmed() const; + bool isKillSwitch() const; + ControlMode getControlMode() const; +}; +#pragma pack(pop) +} // namespace crsf_bridge diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_serial_port.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_serial_port.h new file mode 100644 index 00000000..1db9fd29 --- /dev/null +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_serial_port.h @@ -0,0 +1,48 @@ +#pragma once + +#include +#include + +#include "kr_betaflight_interface/crsf/crsf_msg.h" + +namespace crsf_bridge { + +class CrsfSerialPort { + public: + CrsfSerialPort(); + CrsfSerialPort(const std::string& port, + const rclcpp::Clock::SharedPtr& clock = nullptr); + virtual ~CrsfSerialPort(); + +public: + bool setUpCrsfSerialPort(const std::string& port, + const rclcpp::Clock::SharedPtr& clock = nullptr); + + bool connectSerialPort(const std::string& port); + void disconnectSerialPort(); + + bool startReceiverThread(); + bool stopReceiverThread(); + + void transmitSerialCrsfMessage(const CrsfMsg& crsf_msg) const; + virtual void handleReceivedCrsfMessage( + const crsf_bridge::CrsfMsg& received_crsf_msg) {} + + private: + static constexpr int kCrsfFrameLength_ = 24; // CRSF frame length (update as needed) + static constexpr uint8_t kCrsfHeaderByte_ = 0xC8; // CRSF header (update as needed) + static constexpr int kPollTimeoutMilliSeconds_ = 500; + + bool configureSerialPortForCrsf() const; + void serialPortReceiveThread(); + crsf_bridge::CrsfMsg parseCrsfMessage(const uint8_t* payload) const; + + std::thread receiver_thread_; + std::atomic_bool receiver_thread_should_exit_; + + int serial_port_fd_; + rclcpp::Logger logger_ = rclcpp::get_logger("crsf_serial_port"); + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace crsf_bridge diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/protocol_bridge_base.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/protocol_bridge_base.h new file mode 100644 index 00000000..c2939167 --- /dev/null +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/protocol_bridge_base.h @@ -0,0 +1,16 @@ +#pragma once +#include +#include +#include +#include +#include "kr_betaflight_interface/protocol_msg_base.h" + +class ProtocolBridgeBase { +public: + virtual ~ProtocolBridgeBase() = default; + virtual void controlCommandCallback(const kr_mav_msgs::msg::SO3Command::ConstPtr &msg, const Eigen::Quaterniond &odom_q) = 0; + virtual void armBridge() = 0; + virtual void disarmBridge() = 0; + virtual bool isBridgeArmed() const = 0; + virtual const ProtocolMsgBase* getLastProtocolMsg() const = 0; +}; \ No newline at end of file diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/protocol_msg_base.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/protocol_msg_base.h new file mode 100644 index 00000000..fec64518 --- /dev/null +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/protocol_msg_base.h @@ -0,0 +1,9 @@ +#pragma once +#include + +class ProtocolMsgBase { +public: + virtual ~ProtocolMsgBase() = default; + virtual uint16_t getChannel(int idx) const = 0; + virtual void setChannel(int idx, uint16_t value) = 0; +}; \ No newline at end of file diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_bridge.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_bridge.h new file mode 100644 index 00000000..411a14f9 --- /dev/null +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_bridge.h @@ -0,0 +1,152 @@ +#pragma once + +#include +#include "kr_betaflight_interface/sbus/sbus_msg.h" +#include "kr_betaflight_interface/protocol_bridge_base.h" +#include "kr_betaflight_interface/sbus/sbus_serial_port.h" +#include +#include +#include + +#include +#include +#include +#include + +namespace sbus_bridge +{ + +enum class BridgeState +{ + OFF, + ARMING, + AUTONOMOUS_FLIGHT, + RC_FLIGHT, + KILL +}; + +class SBusBridge : public ProtocolBridgeBase +{ + public: + SBusBridge(const rclcpp::Node::SharedPtr &node); + void controlCommandCallback(const kr_mav_msgs::msg::SO3Command::ConstPtr &msg, const Eigen::Quaterniond &odom_q) override; + void armBridge() override; + void disarmBridge() override; + bool isBridgeArmed() const override; + const ProtocolMsgBase* getLastProtocolMsg() const override { return last_msg_.get(); } + virtual ~SBusBridge(); + + private: + void watchdogThread(); + + void handleReceivedSbusMessage(const SBusMsg &received_sbus_msg); + + SBusMsg generateSBusMessageFromSO3Command(const kr_mav_msgs::msg::SO3Command::ConstPtr &control_command, + const Eigen::Quaterniond &odom_q) const; + + void sendSBusMessageToSerialPort(const SBusMsg &sbus_msg); + + void setBridgeState(const BridgeState &desired_bridge_state); + + double thrust_model_kartik(double thrust) const; + + bool loadParameters(); + + // Serial port interface + std::unique_ptr serial_port_; + std::unique_ptr last_msg_; + // rclcpp Node + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + + // Mutex for: + // - bridge_state_ + // - control_mode_ + // - bridge_armed_ + // - time_last_active_control_command_received_ + // - time_last_rc_msg_received_ + // - arming_counter_ + // - time_last_sbus_msg_sent_ + // Also "setBridgeState" and "sendSBusMessageToSerialPort" should only be + // called when "main_mutex_" is locked + mutable std::mutex main_mutex_; + mutable std::mutex arm_mutex_; + // Mutex for: + // - battery_voltage_ + // - time_last_battery_voltage_received_ + // Also "generateSBusMessageFromControlCommand" should only be called when + // "battery_voltage_mutex_" is locked + mutable std::mutex battery_voltage_mutex_; + + // Subscribers + rclcpp::Subscription::SharedPtr control_command_sub_; + rclcpp::Subscription::SharedPtr battery_voltage_sub_; + // ROS 2: arm_bridge_sub_ removed unless you have a custom message/type for it + + // Timer + rclcpp::TimerBase::SharedPtr low_level_feedback_pub_timer_; + + // Watchdog + std::thread watchdog_thread_; + std::atomic_bool stop_watchdog_thread_; + rclcpp::Time time_last_rc_msg_received_; + rclcpp::Time time_last_sbus_msg_sent_; + rclcpp::Time time_last_battery_voltage_received_; + rclcpp::Time time_last_active_control_command_received_; + + BridgeState bridge_state_; + ControlMode control_mode_; + int arming_counter_; + double battery_voltage_; + + // Safety flags + bool bridge_armed_; + bool rc_was_disarmed_once_; + + std::atomic_bool destructor_invoked_; + + // Parameters + std::string port_name_; + bool enable_receiving_sbus_messages_; + + double control_command_timeout_; + double rc_timeout_; + + double mass_; + + bool disable_thrust_mapping_; + + double max_roll_rate_; + double max_pitch_rate_; + double max_yaw_rate_; + + double max_roll_angle_; + double max_pitch_angle_; + + double alpha_vbat_filter_; + bool perform_thrust_voltage_compensation_; + int n_lipo_cells_; + + // Constants + static constexpr double kLowLevelFeedbackPublishFrequency_ = 50.0; + + static constexpr int kSmoothingFailRepetitions_ = 5; + + static constexpr double kBatteryLowVoltagePerCell_ = 3.6; + static constexpr double kBatteryCriticalVoltagePerCell_ = 3.4; + static constexpr double kBatteryInvalidVoltagePerCell_ = 3.0; + static constexpr double kBatteryVoltageTimeout_ = 1.0; + + double thrust_vs_rpm_cof_a_; + double thrust_vs_rpm_cof_b_; + double thrust_vs_rpm_cof_c_; + double rpm_vs_throttle_linear_coeff_a_; + double rpm_vs_throttle_linear_coeff_b_; + double rpm_vs_throttle_quadratic_coeff_a_; + double rpm_vs_throttle_quadratic_coeff_b_; + double rpm_vs_throttle_quadratic_coeff_c_; + + bool use_body_rates_; +}; + +} // namespace sbus_bridge diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_channel_mapping.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_channel_mapping.h new file mode 100644 index 00000000..32d88331 --- /dev/null +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_channel_mapping.h @@ -0,0 +1,19 @@ +#pragma once +#include + +namespace sbus_bridge { + +namespace sbus_channel_mapping { + +static constexpr uint8_t kThrottle = 2; +static constexpr uint8_t kRoll = 0; +static constexpr uint8_t kPitch = 1; +static constexpr uint8_t kYaw = 3; +static constexpr uint8_t kArming = 4; +static constexpr uint8_t kKillSwitch = 7; +static constexpr uint8_t kControlMode = 5; +static constexpr uint8_t kGamepadMode = 6; + +} // namespace channel_mapping + +} // namespace sbus_bridge diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_msg.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_msg.h new file mode 100644 index 00000000..d64df676 --- /dev/null +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_msg.h @@ -0,0 +1,64 @@ +#pragma once +#include +#include +#include "kr_betaflight_interface/protocol_msg_base.h" + +namespace sbus_bridge { + +enum class ControlMode { NONE, ATTITUDE, BODY_RATES }; +enum class ArmState { DISARMED, ARMED }; + +#pragma pack(push) +#pragma pack(1) +struct SBusMsg : public ProtocolMsgBase { + // Constants + static constexpr int kNChannels = 16; + static constexpr uint16_t kMinCmd = 174; // corresponds to 1000 on FC + static constexpr uint16_t kMeanCmd = 992; // corresponds to 1500 on FC + static constexpr uint16_t kMaxCmd = 1811; // corresponds to 2000 on FC + + rclcpp::Time timestamp; + + // Normal 11 bit channels + uint16_t channels[kNChannels]; + + // Digital channels (ch17 and ch18) + bool digital_channel_1; + bool digital_channel_2; + + // Flags + bool frame_lost; + bool failsafe; + + + SBusMsg(); + virtual ~SBusMsg(); + + uint16_t getChannel(int idx) const override { return channels[idx]; } + void setChannel(int idx, uint16_t value) override { channels[idx] = value; } + +// sbus_bridge::SbusRosMessage toRosMessage() const; + + void limitAllChannelsFeasible(); + void limitSbusChannelFeasible(const int channel_idx); + + // Setting sbus command helpers + void setThrottleCommand(const uint16_t throttle_cmd); + void setRollCommand(const uint16_t roll_cmd); + void setPitchCommand(const uint16_t pitch_cmd); + void setYawCommand(const uint16_t yaw_cmd); + void setControlMode(const ControlMode& control_mode); + void setControlModeAttitude(); + void setControlModeBodyRates(); + void setArmState(const ArmState& arm_state); + void setArmStateArmed(); + void setArmStateDisarmed(); + + // Sbus message check helpers + bool isArmed() const; + bool isKillSwitch() const; + ControlMode getControlMode() const; +}; +#pragma pack(pop) + +} // namespace sbus_bridge diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_serial_port.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_serial_port.h new file mode 100644 index 00000000..8ba17d77 --- /dev/null +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_serial_port.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include "kr_betaflight_interface/sbus/sbus_msg.h" + +namespace sbus_bridge { + +class SBusSerialPort { + public: + SBusSerialPort(); + SBusSerialPort(const std::string& port, + const rclcpp::Clock::SharedPtr& clock = nullptr); + virtual ~SBusSerialPort(); + +public: + bool setUpSBusSerialPort(const std::string& port, + const rclcpp::Clock::SharedPtr& clock = nullptr); + + bool connectSerialPort(const std::string& port); + void disconnectSerialPort(); + + bool startReceiverThread(); + bool stopReceiverThread(); + + void transmitSerialSBusMessage(const SBusMsg& sbus_msg) const; + virtual void handleReceivedSbusMessage( + const sbus_bridge::SBusMsg& received_sbus_msg) {} + + private: + static constexpr int kSbusFrameLength_ = 25; + static constexpr uint8_t kSbusHeaderByte_ = 0x0F; + static constexpr uint8_t kSbusFooterByte_ = 0x00; + static constexpr int kPollTimeoutMilliSeconds_ = 500; + + bool configureSerialPortForSBus() const; + void serialPortReceiveThread(); + sbus_bridge::SBusMsg parseSbusMessage( + uint8_t sbus_msg_bytes[kSbusFrameLength_]) const; + + std::thread receiver_thread_; + std::atomic_bool receiver_thread_should_exit_; + + int serial_port_fd_; + rclcpp::Logger logger_ = rclcpp::get_logger("sbus_serial_port"); + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace sbus_bridge diff --git a/interfaces/kr_betaflight_interface/launch/betaflight_interface.launch.py b/interfaces/kr_betaflight_interface/launch/betaflight_interface.launch.py new file mode 100644 index 00000000..586269a8 --- /dev/null +++ b/interfaces/kr_betaflight_interface/launch/betaflight_interface.launch.py @@ -0,0 +1,57 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + # Declare launch arguments + robot_ns = LaunchConfiguration('robot', default='/') + odom_topic = LaunchConfiguration('odom', default='odom') + so3_cmd_topic = LaunchConfiguration('so3_cmd', default='so3_cmd') + + # Path to the configuration file + config_file = os.path.join( + get_package_share_directory('kr_betaflight_interface'), + 'config', + 'neurofly.yaml' + ) + + # Component container to load your SO3CmdToCRSF component + crsf_container = ComposableNodeContainer( + name='so3cmd_to_crsf_container', + namespace=robot_ns, + package='rclcpp_components', + executable='component_container_mt', # multi-threaded + composable_node_descriptions=[ + ComposableNode( + package='kr_betaflight_interface', + plugin='SO3CmdToBetaflight', + name='so3cmd_to_betaflight', + remappings=[ + ('odom', odom_topic), + ('so3_cmd', so3_cmd_topic), + ], + parameters=[config_file], + ), + ], + output='screen', + ) + robot_arg = DeclareLaunchArgument( + 'robot', default_value='' + ) + odom_arg = DeclareLaunchArgument( + 'odom', default_value='odom' + ) + so3_cmd_arg = DeclareLaunchArgument( + 'so3_cmd', default_value='so3_cmd' + ) + return LaunchDescription([ + robot_arg, + odom_arg, + so3_cmd_arg, + crsf_container, + ]) diff --git a/interfaces/kr_betaflight_interface/launch/control.launch.py b/interfaces/kr_betaflight_interface/launch/control.launch.py new file mode 100644 index 00000000..7acf1813 --- /dev/null +++ b/interfaces/kr_betaflight_interface/launch/control.launch.py @@ -0,0 +1,213 @@ +import os +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, GroupAction, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node, ComposableNodeContainer, PushRosNamespace, LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch.conditions import LaunchConfigurationEquals, IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression, Command, TextSubstitution +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + # KR interface paths & configurations + config_file = os.path.join( + get_package_share_directory('kr_crsf_interface'), + 'config', + 'neurofly.yaml' + ) + + trackers_manager_config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml' + so3_config_file = os.path.join( + get_package_share_directory('kr_crsf_interface'), + 'config', + 'gains.yaml' + ) + + # URDF/xacro file to be loaded by the Robot State Publisher node + default_xacro_path = os.path.join( + get_package_share_directory('zed_wrapper'), + 'urdf', + 'zed_descr.urdf.xacro' + ) + + # Define launch arguments + vicon_args = [ + # Adding mocap vicon specific parameters + DeclareLaunchArgument('mocap', default_value='false'), + DeclareLaunchArgument('mocap_server', default_value='mocap.perch'), + DeclareLaunchArgument('mocap_frame_rate', default_value='100'), + DeclareLaunchArgument('mocap_max_accel', default_value='10.0'), + ] + + # KR interface arguments + kr_args = [ + DeclareLaunchArgument('robot', default_value='neurofly1'), + DeclareLaunchArgument('odom', default_value='odom'), + DeclareLaunchArgument('so3_cmd', default_value='so3_cmd'), + DeclareLaunchArgument('mass', default_value='.680'), + ] + + # ZED camera arguments + zed_args = [ + DeclareLaunchArgument('zed_enable', default_value='true'), + DeclareLaunchArgument('camera_name', default_value='zed'), + DeclareLaunchArgument('camera_model', default_value='zedm'), + DeclareLaunchArgument('publish_urdf', default_value='true'), + DeclareLaunchArgument('publish_tf', default_value='true'), + DeclareLaunchArgument('publish_map_tf', default_value='true'), + DeclareLaunchArgument('publish_imu_tf', default_value='true'), + DeclareLaunchArgument('xacro_path', default_value=TextSubstitution(text=default_xacro_path)), + DeclareLaunchArgument('custom_baseline', default_value='0.0'), + DeclareLaunchArgument('enable_gnss', default_value='false'), + DeclareLaunchArgument('publish_svo_clock', default_value='false'), + ] + + # Initialize launch description with all arguments + ld = LaunchDescription(vicon_args + kr_args + zed_args) + + # Create a main component container for all composable nodes + main_container = ComposableNodeContainer( + name="control_container", + namespace="", # Empty namespace for container, individual nodes will have their own namespaces + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="kr_crsf_interface", + plugin="SO3CmdToCRSF", + name="so3cmd_to_crsf", + namespace=LaunchConfiguration('robot'), + parameters=[config_file], + remappings=[ + ('so3_cmd', LaunchConfiguration('so3_cmd')), + ('odom', LaunchConfiguration('odom')), + ('imu', 'zed/zed_node/imu/data'), + ] + ), + ComposableNode( + package="kr_trackers_manager", + plugin="TrackersManager", + name="trackers_manager", + namespace=LaunchConfiguration('robot'), + parameters=[trackers_manager_config_file], + ), + ComposableNode( + package="kr_trackers_manager", + plugin="TrackersManagerLifecycleManager", + name="lifecycle_manager", + namespace=LaunchConfiguration('robot'), + parameters=[ + {'node_name': "trackers_manager"} + ] + ), + ComposableNode( + package="kr_mav_controllers", + plugin="SO3ControlComponent", + name="so3_controller", + namespace=LaunchConfiguration('robot'), + parameters=[so3_config_file], + ), + ComposableNode( + condition=IfCondition(LaunchConfiguration('zed_enable')), + package="zed_components", + plugin="stereolabs::ZedCamera", + name="zed_node", + namespace="zed", + parameters=[ + [FindPackageShare('zed_wrapper').find('zed_wrapper'), '/config/', LaunchConfiguration('camera_model'), '.yaml'], + [FindPackageShare('zed_wrapper').find('zed_wrapper'), '/config/common_stereo.yaml'], + # Finally apply launch-specific overrides + { + 'general.camera_name': LaunchConfiguration('camera_name'), + 'general.camera_model': LaunchConfiguration('camera_model'), + 'pos_tracking.publish_tf': LaunchConfiguration('publish_tf'), + 'pos_tracking.publish_map_tf': LaunchConfiguration('publish_map_tf'), + 'sensors.publish_imu_tf': LaunchConfiguration('publish_imu_tf', default='true'), + } + ], + extra_arguments=[{'use_intra_process_comms': True}] + ), + ComposableNode( + package="quadrotor_ukf_ros2", + plugin="QuadrotorUKFNode", + name="quadrotor_ukf_ros2", + namespace=LaunchConfiguration('robot'), + parameters=[{ + 'odom': LaunchConfiguration('odom'), + 'imu_frame_id': 'zed_imu_link', + 'imu_rotated_frame_id': 'zed_camera_link', + 'base_link': 'zed_camera_link' + }], + remappings=[ + ('odom', '/zed/zed_node/odom'), + ('imu', '/zed/zed_node/imu/data'), + ], + ), + ], + output='screen', + ) + + ld.add_action(main_container) + + # robot state publisher to publish URDF and static transforms for zed + ld.add_action( + Node( + condition=IfCondition(LaunchConfiguration('zed_enable')), + package='robot_state_publisher', + executable='robot_state_publisher', + name=[LaunchConfiguration('camera_name'), '_state_publisher'], + namespace=LaunchConfiguration('robot'), + output='screen', + parameters=[{ + 'use_sim_time': LaunchConfiguration('publish_svo_clock'), + 'robot_description': Command([ + 'xacro', ' ', LaunchConfiguration('xacro_path'), + ' camera_name:=', LaunchConfiguration('camera_name'), + ' camera_model:=', LaunchConfiguration('camera_model'), + ' custom_baseline:=', LaunchConfiguration('custom_baseline') + ]) + }] + ) + ) + + ld.add_action( + Node( + condition=IfCondition(LaunchConfiguration('mocap')), + package='mocap_vicon', + executable='mocap_vicon_node', + name='vicon', + output='screen', + parameters=[ + {'server_address': LaunchConfiguration('mocap_server')}, + {'frame_rate': LaunchConfiguration('mocap_frame_rate')}, + {'max_accel': LaunchConfiguration('mocap_max_accel')}, + {'publish_tf': False}, + {'publish_pts': False}, + {'fixed_frame_id': 'mocap'}, + # Set to [''] to take in ALL models from Vicon + {'model_list': ['neurofly1']}, + ], + remappings=[ + # Uncomment and modify the remapping if needed + ('/neurofly1/odom', '/odom'), + ] + ) + ) + + ld.add_action( + Node( + package="kr_mav_manager", + executable="mav_services", + namespace=LaunchConfiguration('robot'), + name="mav_services", + output='screen', + parameters = [ + {'mass': LaunchConfiguration('mass')}, + ], + ) + ) + + return ld diff --git a/interfaces/kr_betaflight_interface/package.xml b/interfaces/kr_betaflight_interface/package.xml new file mode 100644 index 00000000..de3c451e --- /dev/null +++ b/interfaces/kr_betaflight_interface/package.xml @@ -0,0 +1,23 @@ + + + kr_betaflight_interface + 0.0.0 + The kr_betaflight_interface package + + Dexter Ong + + BSD + + ament_cmake + + rclcpp + nav_msgs + sensor_msgs + kr_mav_msgs + tf2 + roscpp + rclcpp_components + + ament_cmake + + diff --git a/interfaces/kr_betaflight_interface/scripts/mavlink_telemetry.py b/interfaces/kr_betaflight_interface/scripts/mavlink_telemetry.py new file mode 100644 index 00000000..6c70a881 --- /dev/null +++ b/interfaces/kr_betaflight_interface/scripts/mavlink_telemetry.py @@ -0,0 +1,35 @@ +import rospy +from pymavlink import mavutil +from sensor_msgs.msg import BatteryState + +def mavlink_connect(port='/dev/ttyUSB0', baudrate=115200): + """Connect to a MAVLink source""" + return mavutil.mavlink_connection(port, baud=baudrate) + +def battery_status_publisher(): + rospy.init_node('battery_status_publisher', anonymous=True) + battery_pub = rospy.Publisher('battery_status', BatteryState, queue_size=10) + rate = rospy.Rate(100) + + # Connect to MAVLink (adjust port and baudrate as necessary) + mav = mavlink_connect(port='/dev/ttyUSB0', baudrate=115200) + + while not rospy.is_shutdown(): + message = mav.recv_match(blocking=True) + if message: + if message.get_type() == "SYS_STATUS": + battery_msg = BatteryState() + battery_msg.voltage = message.voltage_battery / 1000.0 / 4.0 + battery_msg.current = message.current_battery / 100.0 + battery_msg.percentage = message.battery_remaining + + battery_msg.header.stamp = rospy.Time.now() + battery_pub.publish(battery_msg) + + rate.sleep() + +if __name__ == '__main__': + try: + battery_status_publisher() + except rospy.ROSInterruptException: + pass diff --git a/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp b/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp new file mode 100644 index 00000000..1a4737ab --- /dev/null +++ b/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp @@ -0,0 +1,612 @@ +#include + +#include +#include +#include + +namespace crsf_bridge { + +CrsfBridge::CrsfBridge(const rclcpp::Node::SharedPtr& node) + : node_(node), + logger_(node->get_logger()), + stop_watchdog_thread_(false), + time_last_rc_msg_received_(), + time_last_crsf_msg_sent_(node_->now()), + time_last_battery_voltage_received_(node_->now()), + time_last_active_control_command_received_(), + bridge_state_(BridgeState::KILL), + control_mode_(ControlMode::NONE), + arming_counter_(0), + battery_voltage_(0.0), + bridge_armed_(false), + rc_was_disarmed_once_(false), + destructor_invoked_(false) { + serial_port_ = std::make_unique(); + if (!loadParameters()) { + RCLCPP_ERROR(logger_, "Could not load parameters."); + rclcpp::shutdown(); + return; + } + + if (!serial_port_->setUpCrsfSerialPort(port_name_, node->get_clock())) { + rclcpp::shutdown(); + return; + } + + try { + watchdog_thread_ = std::thread(&CrsfBridge::watchdogThread, this); + } catch (...) { + RCLCPP_ERROR( + logger_, + "Could not successfully start watchdog thread. Exiting CrsfBridge."); + rclcpp::shutdown(); + return; + } + + RCLCPP_INFO(logger_, "CrsfBridge initialized"); +} + +CrsfBridge::~CrsfBridge() { + destructor_invoked_ = true; + serial_port_->stopReceiverThread(); + + stop_watchdog_thread_ = true; + watchdog_thread_.join(); + setBridgeState(BridgeState::OFF); + + // Send disarming CRSF message for safety + CrsfMsg shut_down_message; + shut_down_message.setArmStateDisarmed(); + rclcpp::Rate loop_rate(110.0); + for (int i = 0; i < kSmoothingFailRepetitions_; i++) { + serial_port_->transmitSerialCrsfMessage(shut_down_message); + loop_rate.sleep(); + } + + serial_port_->disconnectSerialPort(); +} + +void CrsfBridge::watchdogThread() { + rclcpp::Rate watchdog_rate(110.0); + while (rclcpp::ok() && !stop_watchdog_thread_) { + watchdog_rate.sleep(); + std::lock_guard main_lock(main_mutex_); + const rclcpp::Time time_now = node_->now(); + if (bridge_state_ == BridgeState::RC_FLIGHT && + (time_now - time_last_rc_msg_received_).seconds() > rc_timeout_) { + RCLCPP_WARN( + logger_, + "Remote control was active but no message from it was received " + "within timeout (%f s).", + rc_timeout_); + setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); + } + if (bridge_state_ == BridgeState::KILL) { + CrsfMsg kill_msg; + kill_msg.setArmStateDisarmed(); + sendCrsfMessageToSerialPort(kill_msg); + if (bridge_armed_) { + disarmBridge(); + } + } + std::lock_guard battery_lock(battery_voltage_mutex_); + if ((time_now - time_last_battery_voltage_received_).seconds() > + kBatteryVoltageTimeout_) { + battery_voltage_ = 0.0; + if (perform_thrust_voltage_compensation_) { + RCLCPP_WARN_THROTTLE( + logger_, *node_->get_clock(), 1000, + "Can not perform battery voltage compensation because there " + "is no recent battery voltage measurement"); + } + } + } +} + +void CrsfBridge::handleReceivedCrsfMessage(const CrsfMsg &received_crsf_msg) +{ + { + std::lock_guard main_lock(main_mutex_); + + time_last_rc_msg_received_ = node_->now(); + + if (received_crsf_msg.isKillSwitch()) + { + if (bridge_state_ != BridgeState::KILL) + { + setBridgeState(BridgeState::KILL); + RCLCPP_INFO(logger_, "Kill switch ON"); + } + } + else + { + if (bridge_state_ == BridgeState::KILL) + { + setBridgeState(BridgeState::OFF); + RCLCPP_WARN(logger_, "Kill switch OFF"); + } + } + + if (bridge_state_ == BridgeState::KILL) + { + // send disarm to FC to kill motors + // sendCrsfMessageToSerialPort(received_crsf_msg); + + return; + } + + if (received_crsf_msg.isArmed()) + { + if (!rc_was_disarmed_once_) { + // This flag prevents that the vehicle can be armed if the RC is armed + // on startup of the bridge + RCLCPP_WARN_THROTTLE( + logger_, *node_->get_clock(), 1000, + "RC needs to be disarmed once before it can take over control"); + return; + } + + // Immediately go into RC_FLIGHT state since RC always has priority + if (bridge_state_ != BridgeState::RC_FLIGHT) { + setBridgeState(BridgeState::RC_FLIGHT); + RCLCPP_INFO(logger_, "Control authority taken over by remote control."); + } + sendCrsfMessageToSerialPort(received_crsf_msg); + control_mode_ = received_crsf_msg.getControlMode(); + } + else if (bridge_state_ == BridgeState::RC_FLIGHT) + { + // If the bridge was in state RC_FLIGHT and the RC is disarmed we set the + // state to AUTONOMOUS_FLIGHT + // In case there are valid control commands, the bridge will stay in + // AUTONOMOUS_FLIGHT, otherwise the watchdog will set the state to OFF + RCLCPP_INFO(logger_, "Control authority returned by remote control."); + if (bridge_armed_) + { + RCLCPP_INFO(logger_, "Bridge armed, setting bridge state to AUTONOMOUS_FLIGHT"); + setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); + } + else + { + // When switching the bridge state to off, our watchdog ensures that a + // disarming off message is sent + RCLCPP_INFO(logger_, "Bridge not armed, setting bridge state to OFF"); + setBridgeState(BridgeState::OFF); + } + } + else if (!rc_was_disarmed_once_) + { + RCLCPP_INFO( + logger_, + "Received disarmed RC message but RC was not disarmed once, ignoring it"); + rc_was_disarmed_once_ = true; + } + else if (bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) { + // not killed, not armed, not autonomous flight + sendCrsfMessageToSerialPort(received_crsf_msg); + } + + // Main mutex is unlocked here because it goes out of scope + } + +// received_crsf_msg_pub_.publish(received_crsf_msg.toRosMessage()); +} + +void CrsfBridge::controlCommandCallback( + const kr_mav_msgs::msg::SO3Command::ConstPtr &msg, + const Eigen::Quaterniond &odom_q) +{ + std::lock_guard main_lock(main_mutex_); + if (destructor_invoked_) + { + return; + } + time_last_active_control_command_received_ = node_->now(); + if (!bridge_armed_ || bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) + { + if (!bridge_armed_ && msg->aux.enable_motors && + bridge_state_ != BridgeState::RC_FLIGHT) + { + RCLCPP_WARN( + logger_, + "Received active control command but crsf bridge is not armed. " + "Please arm the bridge before sending control commands."); + } + return; + } + CrsfMsg crsf_msg_to_send; + { + std::lock_guard battery_lock(battery_voltage_mutex_); + crsf_msg_to_send = generateCrsfMessageFromSO3Command(msg, odom_q); + } + if (!msg->aux.enable_motors) + { + if (bridge_state_ == BridgeState::ARMING || + bridge_state_ == BridgeState::AUTONOMOUS_FLIGHT) + { + RCLCPP_INFO(logger_, "Control command received, setting bridge state to OFF"); + setBridgeState(BridgeState::OFF); + } + crsf_msg_to_send.setArmStateDisarmed(); + } + crsf_msg_to_send.limitAllChannelsFeasible(); + sendCrsfMessageToSerialPort(crsf_msg_to_send); +} + +void CrsfBridge::sendCrsfMessageToSerialPort(const CrsfMsg& crsf_msg) +{ + CrsfMsg crsf_message_to_send = crsf_msg; + + switch (bridge_state_) { + case BridgeState::OFF: + // Disarm vehicle + crsf_message_to_send.setArmStateDisarmed(); + break; + + case BridgeState::ARMING: + // Since there might be some RC commands smoothing and checks on multiple + // messages before arming, we repeat the messages with minimum throttle + // and arming command multiple times. 5 times seems to work robustly. + if (arming_counter_ >= kSmoothingFailRepetitions_) { + setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); + } else { + // Set thrust to minimum command to make sure FC arms + crsf_message_to_send.setThrottleCommand(CrsfMsg::kMinCmd); + crsf_message_to_send.setArmStateArmed(); + arming_counter_++; + } + break; + + case BridgeState::AUTONOMOUS_FLIGHT: + crsf_message_to_send.setArmStateArmed(); + if (use_body_rates_) + { + crsf_message_to_send.setControlModeBodyRates(); + } else + { + crsf_message_to_send.setControlModeAttitude(); + } + break; + + case BridgeState::RC_FLIGHT: + // Passing RC command straight through + // RCLCPP_INFO(logger_, "RC MODE: throttle_cmd %d", crsf_message_to_send.channels[crsf_bridge::crsf_channel_mapping::kThrottle]); + crsf_message_to_send.setControlModeAttitude(); + break; + + case BridgeState::KILL: + // Disarm vehicle + crsf_message_to_send.setArmStateDisarmed(); + break; + + default: + // Disarm the vehicle because this code must be terribly wrong + crsf_message_to_send.setArmStateDisarmed(); + RCLCPP_WARN( + logger_, "Bridge is in unknown state, vehicle will be disarmed"); + break; + } + + if ((node_->now() - time_last_crsf_msg_sent_).seconds() <= 0.006) { + // An CRSF message takes 3ms to be transmitted by the serial port so let's + // not stress it too much. This should only happen in case of switching + // between control commands and rc commands + if (bridge_state_ == BridgeState::ARMING) { + // In case of arming we want to send kSmoothingFailRepetitions_ messages + // with minimum throttle to the flight controller. Since this check + // prevents the message from being sent out we reduce the counter that + // was incremented above assuming the message would actually be sent. + arming_counter_--; + } + return; + } + + crsf_message_to_send.timestamp = node_->now(); + serial_port_->transmitSerialCrsfMessage(crsf_message_to_send); + time_last_crsf_msg_sent_ = node_->now(); +} + +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)); +} + +double CrsfBridge::thrust_model_kartik(double thrust) const +{ + int num_props = 4; + double avg_thrust = std::max(0.0, thrust) / num_props; + + // Scale thrust to individual rotor velocities (RPM) + 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 betaflight + double throttle = + (omega_avg - rpm_vs_throttle_linear_coeff_b_) / rpm_vs_throttle_linear_coeff_a_; + return throttle; +} + +CrsfMsg CrsfBridge::generateCrsfMessageFromSO3Command(const kr_mav_msgs::msg::SO3Command::ConstPtr& msg, const Eigen::Quaterniond& odom_q) const +{ + CrsfMsg crsf_msg; + + // set crsf_msg to not killed + crsf_msg.channels[crsf_bridge::crsf_channel_mapping::kKillSwitch] = 1792; + + crsf_msg.setArmStateArmed(); + + // 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); + + const Eigen::Matrix3d R_cur(odom_q); + + double thrust = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); + + double throttle = 0.0; + if (thrust > 1e-5) + { + throttle = thrust_model_kartik(thrust); + } + + // 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); + crsf_msg.setThrottleCommand(throttle_cmd); + + // convert quaternion to euler + Eigen::Vector3d desired_euler_angles = q_des.toRotationMatrix().eulerAngles(0, 1, 2); + + // sanity check if larger than 3 rad, set to 0 + for (int i = 0; i < 3; i++) { + if (desired_euler_angles(i) > 3.0 || desired_euler_angles(i) < -3.0) + { + desired_euler_angles(i) = 0; + } + } + + // parse commands + uint16_t roll_cmd, pitch_cmd, yaw_cmd; + + if (use_body_rates_) + { + // get body rates + double roll_rate = msg->angular_velocity.x; + double pitch_rate = msg->angular_velocity.y; + + roll_rate = std::max(-max_roll_rate_, std::min(max_roll_rate_, roll_rate)); + pitch_rate = std::max(-max_pitch_rate_, std::min(max_pitch_rate_, pitch_rate)); + + // additional safety check + roll_rate = std::max(-400.0, std::min(400.0, roll_rate)); + pitch_rate = std::max(-400.0, std::min(400.0, pitch_rate)); + + roll_cmd = round((roll_rate + max_roll_rate_) / (2 * max_roll_rate_) * (CrsfMsg::kMaxCmd - CrsfMsg::kMinCmd) + CrsfMsg::kMinCmd); + pitch_cmd = round((pitch_rate + max_pitch_rate_) / (2 * max_pitch_rate_) * (CrsfMsg::kMaxCmd - CrsfMsg::kMinCmd) + CrsfMsg::kMinCmd); + + RCLCPP_INFO_THROTTLE( + logger_, *node_->get_clock(), 1000, + "AUTONOMOUS MODE: roll_rate: %f pitch_rate: %f roll_cmd: %d pitch_cmd: %d", + roll_rate, pitch_rate, roll_cmd, pitch_cmd); + } + else + { + // get body angles + double yaw, roll, pitch; + // use tf2 to convert quaternion to euler + tf2::Matrix3x3(tf2::Quaternion(q_des.x(), q_des.y(), q_des.z(), q_des.w())).getRPY(roll, pitch, yaw); + // convert to degrees + double roll_deg = roll * 180.0 / M_PI; + double pitch_deg = pitch * 180.0 / M_PI; + + // clip max roll + roll_deg = std::max(-max_roll_angle_, std::min(max_roll_angle_, roll_deg)); + // clip max pitch + pitch_deg = std::max(-max_pitch_angle_, std::min(max_pitch_angle_, pitch_deg)); + + roll_cmd = round((roll_deg + max_roll_angle_) / (2 * max_roll_angle_) * (CrsfMsg::kMaxCmd - CrsfMsg::kMinCmd) + CrsfMsg::kMinCmd); + pitch_cmd = round((pitch_deg + max_pitch_angle_) / (2 * max_pitch_angle_) * (CrsfMsg::kMaxCmd - CrsfMsg::kMinCmd) + CrsfMsg::kMinCmd); + + RCLCPP_INFO_THROTTLE( + logger_, *node_->get_clock(), 1000, + "AUTONOMOUS MODE: roll_deg: %f pitch_deg: %f roll_cmd: %d pitch_cmd: %d", + roll_deg, pitch_deg, roll_cmd, pitch_cmd); + } + + // always use yaw rate + double yaw_rate = -msg->angular_velocity.z; // flip sign since FC yaw is inverted (z-down) + yaw_rate = yaw_rate * 180.0 / M_PI; // convert to degrees + yaw_rate = std::max(-400.0, std::min(400.0, yaw_rate)); // clip max yaw rate + yaw_cmd = round((yaw_rate + max_yaw_rate_) / (2 * max_yaw_rate_) * (CrsfMsg::kMaxCmd - CrsfMsg::kMinCmd) + CrsfMsg::kMinCmd); + + RCLCPP_INFO_THROTTLE( + logger_, *node_->get_clock(), 1000, + "AUTONOMOUS MODE: yaw_rate: %f yaw_cmd: %d", + yaw_rate, yaw_cmd); + + crsf_msg.setRollCommand(roll_cmd); + crsf_msg.setPitchCommand(pitch_cmd); + crsf_msg.setYawCommand(yaw_cmd); + + return crsf_msg; +} + +void CrsfBridge::setBridgeState(const BridgeState& desired_bridge_state) { + switch (desired_bridge_state) { + case BridgeState::OFF: + bridge_state_ = desired_bridge_state; + break; + + case BridgeState::ARMING: + bridge_state_ = desired_bridge_state; + arming_counter_ = 0; + break; + + case BridgeState::AUTONOMOUS_FLIGHT: + bridge_state_ = desired_bridge_state; + break; + + case BridgeState::RC_FLIGHT: + bridge_state_ = desired_bridge_state; + break; + + case BridgeState::KILL: + bridge_state_ = desired_bridge_state; + break; + + default: + RCLCPP_WARN( + logger_, "Wanted to switch to unknown bridge state, setting to OFF"); + bridge_state_ = BridgeState::OFF; + } +} + +void CrsfBridge::armBridge() +{ + std::lock_guard main_lock(arm_mutex_); + bridge_armed_ = true; +} + +void CrsfBridge::disarmBridge() +{ + std::lock_guard main_lock(arm_mutex_); + bridge_armed_ = false; +} + +bool CrsfBridge::isBridgeArmed() const +{ + return bridge_armed_; +} + +bool CrsfBridge::loadParameters() +{ + node_->declare_parameter("port_name", std::string("/dev/ttyUSB0")); + node_->declare_parameter("control_command_timeout", 0.5); + node_->declare_parameter("rc_timeout", 0.5); + node_->declare_parameter("mass", 1.0); + node_->declare_parameter("disable_thrust_mapping", false); + node_->declare_parameter("max_roll_rate", 400.0); + node_->declare_parameter("max_pitch_rate", 400.0); + node_->declare_parameter("max_yaw_rate", 400.0); + node_->declare_parameter("max_roll_angle", 0.5); + node_->declare_parameter("max_pitch_angle", 0.5); + node_->declare_parameter("alpha_vbat_filter", 0.1); + node_->declare_parameter("perform_thrust_voltage_compensation", true); + node_->declare_parameter("n_lipo_cells", 3); + node_->declare_parameter("thrust_vs_rpm_cof_a_", 0.0001); + node_->declare_parameter("thrust_vs_rpm_cof_b_", 0.0); + node_->declare_parameter("thrust_vs_rpm_cof_c_", 0.0); + node_->declare_parameter("rpm_vs_throttle_linear_coeff_a_", 0.0001); + node_->declare_parameter("rpm_vs_throttle_linear_coeff_b_", 0.0); + node_->declare_parameter("rpm_vs_throttle_quadratic_coeff_a_", 0.0001); + node_->declare_parameter("rpm_vs_throttle_quadratic_coeff_b_", 0.0); + node_->declare_parameter("rpm_vs_throttle_quadratic_coeff_c_", 0.0); + node_->declare_parameter("use_body_rates", false); + + // Get parameters + bool all_params_loaded = true; + + if (!node_->get_parameter("port_name", port_name_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: port_name"); + all_params_loaded = false; + } + if (!node_->get_parameter("control_command_timeout", control_command_timeout_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: control_command_timeout"); + all_params_loaded = false; + } + if (!node_->get_parameter("rc_timeout", rc_timeout_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: rc_timeout"); + all_params_loaded = false; + } + if (!node_->get_parameter("mass", mass_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: mass"); + all_params_loaded = false; + } + if (!node_->get_parameter("disable_thrust_mapping", disable_thrust_mapping_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: disable_thrust_mapping"); + all_params_loaded = false; + } + if (!node_->get_parameter("max_roll_rate", max_roll_rate_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: max_roll_rate"); + all_params_loaded = false; + } + if (!node_->get_parameter("max_pitch_rate", max_pitch_rate_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: max_pitch_rate"); + all_params_loaded = false; + } + if (!node_->get_parameter("max_yaw_rate", max_yaw_rate_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: max_yaw_rate"); + all_params_loaded = false; + } + if (!node_->get_parameter("max_roll_angle", max_roll_angle_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: max_roll_angle"); + all_params_loaded = false; + } + if (!node_->get_parameter("max_pitch_angle", max_pitch_angle_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: max_pitch_angle"); + all_params_loaded = false; + } + if (!node_->get_parameter("alpha_vbat_filter", alpha_vbat_filter_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: alpha_vbat_filter"); + all_params_loaded = false; + } + if (!node_->get_parameter("perform_thrust_voltage_compensation", perform_thrust_voltage_compensation_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: perform_thrust_voltage_compensation"); + all_params_loaded = false; + } + if (!node_->get_parameter("n_lipo_cells", n_lipo_cells_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: n_lipo_cells"); + all_params_loaded = false; + } + if (!node_->get_parameter("thrust_vs_rpm_cof_a_", thrust_vs_rpm_cof_a_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: thrust_vs_rpm_cof_a_"); + all_params_loaded = false; + } + if (!node_->get_parameter("thrust_vs_rpm_cof_b_", thrust_vs_rpm_cof_b_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: thrust_vs_rpm_cof_b_"); + all_params_loaded = false; + } + if (!node_->get_parameter("thrust_vs_rpm_cof_c_", thrust_vs_rpm_cof_c_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: thrust_vs_rpm_cof_c_"); + all_params_loaded = false; + } + if (!node_->get_parameter("rpm_vs_throttle_linear_coeff_a_", rpm_vs_throttle_linear_coeff_a_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: rpm_vs_throttle_linear_coeff_a_"); + all_params_loaded = false; + } + if (!node_->get_parameter("rpm_vs_throttle_linear_coeff_b_", rpm_vs_throttle_linear_coeff_b_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: rpm_vs_throttle_linear_coeff_b_"); + all_params_loaded = false; + } + if (!node_->get_parameter("rpm_vs_throttle_quadratic_coeff_a_", rpm_vs_throttle_quadratic_coeff_a_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: rpm_vs_throttle_quadratic_coeff_a_"); + all_params_loaded = false; + } + if (!node_->get_parameter("rpm_vs_throttle_quadratic_coeff_b_", rpm_vs_throttle_quadratic_coeff_b_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: rpm_vs_throttle_quadratic_coeff_b_"); + all_params_loaded = false; + } + if (!node_->get_parameter("rpm_vs_throttle_quadratic_coeff_c_", rpm_vs_throttle_quadratic_coeff_c_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: rpm_vs_throttle_quadratic_coeff_c_"); + all_params_loaded = false; + } + if (!node_->get_parameter("use_body_rates", use_body_rates_)) { + RCLCPP_ERROR(logger_, "Failed to load parameter: use_body_rates"); + all_params_loaded = false; + } + + if (!all_params_loaded) { + RCLCPP_ERROR(logger_, "Failed to load one or more parameters."); + return false; + } + return true; +} + +} // namespace crsf_bridge diff --git a/interfaces/kr_betaflight_interface/src/crsf/crsf_msg.cpp b/interfaces/kr_betaflight_interface/src/crsf/crsf_msg.cpp new file mode 100644 index 00000000..50c9745e --- /dev/null +++ b/interfaces/kr_betaflight_interface/src/crsf/crsf_msg.cpp @@ -0,0 +1,103 @@ +#include "kr_betaflight_interface/crsf/crsf_msg.h" + +#include "kr_betaflight_interface/crsf/crsf_channel_mapping.h" + +namespace crsf_bridge { + +CrsfMsg::CrsfMsg() + : timestamp(rclcpp::Clock().now()), + digital_channel_1(false), + digital_channel_2(false), + frame_lost(false), + failsafe(false) { + for (int i = 0; i < kNChannels; i++) { + channels[i] = kMeanCmd; + } +} + +CrsfMsg::~CrsfMsg() {} + +void CrsfMsg::limitAllChannelsFeasible() { + for (uint8_t i = 0; i < kNChannels; i++) { + limitCrsfChannelFeasible(i); + } +} + +void CrsfMsg::limitCrsfChannelFeasible(const int channel_idx) { + if (channel_idx < 0 || channel_idx >= kNChannels) { + return; + } + + if (channels[channel_idx] > kMaxCmd) { + channels[channel_idx] = kMaxCmd; + } + if (channels[channel_idx] < kMinCmd) { + channels[channel_idx] = kMinCmd; + } +} + +void CrsfMsg::setThrottleCommand(const uint16_t throttle_cmd) { + // Update with CRSF channel mapping if needed + channels[crsf_channel_mapping::kThrottle] = throttle_cmd; +} + +void CrsfMsg::setRollCommand(const uint16_t roll_cmd) { + channels[crsf_channel_mapping::kRoll] = roll_cmd; +} + +void CrsfMsg::setPitchCommand(const uint16_t pitch_cmd) { + channels[crsf_channel_mapping::kPitch] = pitch_cmd; +} + +void CrsfMsg::setYawCommand(const uint16_t yaw_cmd) { + channels[crsf_channel_mapping::kYaw] = yaw_cmd; +} + +void CrsfMsg::setControlMode(const ControlMode& control_mode) { + // Update with CRSF-specific logic if needed + if (control_mode == ControlMode::ATTITUDE) { + channels[crsf_channel_mapping::kControlMode] = kMinCmd; + } else if (control_mode == ControlMode::BODY_RATES) { + channels[crsf_channel_mapping::kControlMode] = kMaxCmd; + } +} + +void CrsfMsg::setControlModeAttitude() { + setControlMode(ControlMode::ATTITUDE); +} + +void CrsfMsg::setControlModeBodyRates() { + setControlMode(ControlMode::BODY_RATES); +} + +void CrsfMsg::setArmState(const ArmState& arm_state) { + if (arm_state == ArmState::ARMED) { + channels[crsf_channel_mapping::kArming] = kMaxCmd; + } else { + channels[crsf_channel_mapping::kArming] = kMinCmd; + } +} + +void CrsfMsg::setArmStateArmed() { setArmState(ArmState::ARMED); } + +void CrsfMsg::setArmStateDisarmed() { + setArmState(ArmState::DISARMED); + setThrottleCommand(kMinCmd); +} + +bool CrsfMsg::isArmed() const { + return channels[crsf_channel_mapping::kArming] > kMeanCmd; +} + +bool CrsfMsg::isKillSwitch() const { + return channels[crsf_channel_mapping::kKillSwitch] <= kMeanCmd; +} + +ControlMode CrsfMsg::getControlMode() const { + if (channels[crsf_channel_mapping::kControlMode] > kMeanCmd) { + return ControlMode::BODY_RATES; + } + return ControlMode::ATTITUDE; +} + +} // namespace crsf_bridge diff --git a/interfaces/kr_betaflight_interface/src/crsf/crsf_serial_port.cpp b/interfaces/kr_betaflight_interface/src/crsf/crsf_serial_port.cpp new file mode 100644 index 00000000..cc5e82ad --- /dev/null +++ b/interfaces/kr_betaflight_interface/src/crsf/crsf_serial_port.cpp @@ -0,0 +1,326 @@ +#include "kr_betaflight_interface/crsf/crsf_serial_port.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace crsf_bridge { + +namespace { +constexpr uint8_t CRSF_ADDRESS_FC = 0xC8; +constexpr uint8_t CRSF_TYPE_RC_CHANNELS_PACKED = 0x16; +constexpr size_t CRSF_RC_CHANNELS_PAYLOAD_SIZE = 22; +constexpr size_t CRSF_RC_CHANNELS_FRAME_SIZE = 26; // 1(addr)+1(len)+1(type)+22(payload)+1(crc) + +// CRC-8/MAXIM (poly 0xD5) +uint8_t crsf_crc8(const uint8_t* data, size_t len) { + uint8_t crc = 0; + for (size_t i = 0; i < len; ++i) { + crc ^= data[i]; + for (int j = 0; j < 8; ++j) { + if (crc & 0x80) + crc = (crc << 1) ^ 0xD5; + else + crc <<= 1; + } + } + return crc; +} +} + +CrsfSerialPort::CrsfSerialPort() + : receiver_thread_(), + receiver_thread_should_exit_(false), + serial_port_fd_(-1), + clock_(nullptr) { + logger_ = rclcpp::get_logger("crsf_serial_port"); +} + +CrsfSerialPort::CrsfSerialPort(const std::string& port, + const rclcpp::Clock::SharedPtr& clock) + : receiver_thread_(), + receiver_thread_should_exit_(false), + serial_port_fd_(-1), + clock_(clock) { + logger_ = rclcpp::get_logger("crsf_serial_port"); +} + +CrsfSerialPort::~CrsfSerialPort() { disconnectSerialPort(); } + +bool CrsfSerialPort::setUpCrsfSerialPort(const std::string& port, + const rclcpp::Clock::SharedPtr& clock) { + clock_ = clock; + if (!connectSerialPort(port)) { + return false; + } + + if (!startReceiverThread()) { + return false; + } + + return true; +} + +bool CrsfSerialPort::connectSerialPort(const std::string& port) { + serial_port_fd_ = open(port.c_str(), O_RDWR | O_NOCTTY); + + if (serial_port_fd_ == -1) { + RCLCPP_ERROR(logger_, "Could not open serial port %s", port.c_str()); + return false; + } + + if (!configureSerialPortForCrsf()) { + close(serial_port_fd_); + RCLCPP_ERROR(logger_, "Could not set necessary configuration of serial port"); + return false; + } + + RCLCPP_INFO(logger_, "Connected to serial port %s", port.c_str()); + return true; +} + +void CrsfSerialPort::disconnectSerialPort() { + stopReceiverThread(); + if (serial_port_fd_ != -1) { + close(serial_port_fd_); + serial_port_fd_ = -1; + } +} + +bool CrsfSerialPort::startReceiverThread() { + try { + receiver_thread_ = + std::thread(&CrsfSerialPort::serialPortReceiveThread, this); + } catch (...) { + RCLCPP_ERROR(logger_, "Could not successfully start CRSF receiver thread."); + return false; + } + + return true; +} + +bool CrsfSerialPort::stopReceiverThread() { + if (!receiver_thread_.joinable()) { + return true; + } + + receiver_thread_should_exit_ = true; + receiver_thread_.join(); + return true; +} + +bool CrsfSerialPort::configureSerialPortForCrsf() const { + fcntl(serial_port_fd_, F_SETFL, 0); + fcntl(serial_port_fd_, F_SETFL, FNDELAY); + + struct termios2 uart_config; + ioctl(serial_port_fd_, TCGETS2, &uart_config); + + uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + uart_config.c_cflag &= ~(CSIZE | PARODD | CBAUD); + uart_config.c_cflag |= CS8; + uart_config.c_cflag |= BOTHER; + + // CRSF baud rate is 420000 + const speed_t spd = 420000; + uart_config.c_ispeed = spd; + uart_config.c_ospeed = spd; + RCLCPP_INFO(logger_, "Setting CRSF serial port baud rate to %d", (int)spd); + + if (ioctl(serial_port_fd_, TCSETS2, &uart_config) < 0) { + RCLCPP_ERROR(logger_, "Could not set configuration of serial port"); + return false; + } + + return true; +} + +void CrsfSerialPort::transmitSerialCrsfMessage(const CrsfMsg& crsf_msg) const { + uint8_t frame[CRSF_RC_CHANNELS_FRAME_SIZE] = {0}; + frame[0] = CRSF_ADDRESS_FC; + frame[1] = CRSF_RC_CHANNELS_PAYLOAD_SIZE + 2; // type + payload + crc + frame[2] = CRSF_TYPE_RC_CHANNELS_PACKED; + + // Pack 16 channels (11 bits each) into 22 bytes (LSB first) + uint16_t chans[16]; + for (int i = 0; i < 16; ++i) { + chans[i] = crsf_msg.channels[i]; + } + uint8_t* payload = &frame[3]; + memset(payload, 0, CRSF_RC_CHANNELS_PAYLOAD_SIZE); + + // Proper bit packing for 11-bit channels + uint32_t bit_pos = 0; + for (int ch = 0; ch < 16; ++ch) { + uint16_t val = chans[ch] & 0x07FF; // 11-bit mask + + // Calculate byte and bit positions + uint32_t byte_pos = bit_pos / 8; + uint32_t bit_offset = bit_pos % 8; + + // Write the 11-bit value + if (bit_offset <= 5) { + // Value fits in current byte and part of next byte + payload[byte_pos] |= (val << bit_offset) & 0xFF; + payload[byte_pos + 1] |= (val >> (8 - bit_offset)) & 0xFF; + } else { + // Value spans three bytes + payload[byte_pos] |= (val << bit_offset) & 0xFF; + payload[byte_pos + 1] |= (val >> (8 - bit_offset)) & 0xFF; + payload[byte_pos + 2] |= (val >> (16 - bit_offset)) & 0xFF; + } + + bit_pos += 11; + } + + // CRC over [type, payload] (not address + length) + frame[CRSF_RC_CHANNELS_FRAME_SIZE - 1] = crsf_crc8(&frame[2], CRSF_RC_CHANNELS_FRAME_SIZE - 3); + + int written = write(serial_port_fd_, frame, CRSF_RC_CHANNELS_FRAME_SIZE); + if (written != (int)CRSF_RC_CHANNELS_FRAME_SIZE) { + RCLCPP_ERROR(logger_, "Wrote %d bytes but should have written %zu", written, CRSF_RC_CHANNELS_FRAME_SIZE); + } +} + +void CrsfSerialPort::serialPortReceiveThread() { + struct pollfd fds[1]; + fds[0].fd = serial_port_fd_; + fds[0].events = POLLIN; + std::deque bytes_buf; + + while (!receiver_thread_should_exit_) { + uint8_t read_buf[128]; + if (poll(fds, 1, kPollTimeoutMilliSeconds_) > 0) { + if (fds[0].revents & POLLIN) { + ssize_t nread = read(serial_port_fd_, read_buf, sizeof(read_buf)); + for (ssize_t i = 0; i < nread; ++i) { + bytes_buf.push_back(read_buf[i]); + } + + // Try to extract CRSF frames with proper synchronization + while (bytes_buf.size() >= 3) { // Need at least address + length + type + // Look for any valid CRSF address (0xC8 or 0xEE) + if (bytes_buf[0] == 0xC8 || bytes_buf[0] == 0xEE) { + uint8_t frame_length = bytes_buf[1]; + uint8_t total_frame_size = frame_length + 2; // +2 for address and length bytes + + // Validate frame length and ensure we have the complete frame + if (frame_length >= 2 && frame_length <= 64 && // Reasonable length bounds + bytes_buf.size() >= total_frame_size) { // Wait for complete frame + + // Check if this is an RC channels frame + if (bytes_buf[2] == CRSF_TYPE_RC_CHANNELS_PACKED) { + // Extract the complete frame + uint8_t frame[total_frame_size]; + for (size_t i = 0; i < total_frame_size; ++i) { + frame[i] = bytes_buf[i]; + } + + // Check CRC - only process frames with valid CRC + uint8_t crc = crsf_crc8(&frame[2], frame_length - 1); // CRC over type + payload only + if (crc == frame[frame_length + 1]) { + // Valid frame - parse and process + CrsfMsg msg = parseCrsfMessage(&frame[3]); // Skip address, length, type + if (clock_) msg.timestamp = clock_->now(); + else msg.timestamp = rclcpp::Clock().now(); + + RCLCPP_DEBUG(logger_, "Valid CRSF frame - Address: %02X, Length: %d, Type: %02X", + frame[0], frame[1], frame[2]); + handleReceivedCrsfMessage(msg); + } else { + // CRC mismatch - log and discard + RCLCPP_WARN(logger_, "CRSF frame CRC mismatch - discarding frame"); + } + + // Remove the processed frame (regardless of CRC result) + for (size_t i = 0; i < total_frame_size; ++i) { + bytes_buf.pop_front(); + } + continue; + } else { + // Not an RC channels frame - remove and continue + for (size_t i = 0; i < total_frame_size; ++i) { + bytes_buf.pop_front(); + } + continue; + } + } else { + // Incomplete frame - wait for more data + break; + } + } + // Not a valid frame start, pop one byte and continue searching + bytes_buf.pop_front(); + } + + // Prevent buffer from growing too large + if (bytes_buf.size() > 256) { + RCLCPP_WARN(logger_, "Buffer overflow, clearing buffer"); + bytes_buf.clear(); + } + } + } + } +} + +CrsfMsg CrsfSerialPort::parseCrsfMessage(const uint8_t* payload) const { + CrsfMsg msg; + const unsigned numOfChannels = 16; + const unsigned srcBits = 11; + const unsigned inputChannelMask = (1 << srcBits) - 1; + + // Initialize all channels to safe default values (center position) + for (uint8_t n = 0; n < numOfChannels; n++) { + msg.channels[n] = 1024; // Center position (2048/2) + } + + uint8_t bitsMerged = 0; + uint32_t readValue = 0; + unsigned readByteIndex = 0; + + for (uint8_t n = 0; n < numOfChannels; n++) { + while (bitsMerged < srcBits) { + if (readByteIndex >= CRSF_RC_CHANNELS_PAYLOAD_SIZE) { + RCLCPP_ERROR(logger_, "Payload buffer overflow during channel parsing"); + return msg; // Return safe default values + } + uint8_t readByte = payload[readByteIndex++]; + readValue |= ((uint32_t) readByte) << bitsMerged; + bitsMerged += 8; + } + + uint16_t channelValue = (readValue & inputChannelMask); + + // Validate channel value is within reasonable bounds + if (channelValue <= 2047) { // 11-bit max value + msg.channels[n] = channelValue; + } else { + RCLCPP_WARN(logger_, "Invalid channel %d value: %d, using default", n, channelValue); + msg.channels[n] = 1024; // Center position + } + + readValue >>= srcBits; + bitsMerged -= srcBits; + } + + // No digital channels, frame_lost, or failsafe in CRSF RC frame + msg.digital_channel_1 = false; + msg.digital_channel_2 = false; + msg.frame_lost = false; + msg.failsafe = false; + + return msg; +} + +} // namespace crsf_bridge diff --git a/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp b/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp new file mode 100644 index 00000000..fb8222c8 --- /dev/null +++ b/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp @@ -0,0 +1,636 @@ +#include +#include "kr_betaflight_interface/sbus/sbus_bridge.h" +#include +#include + +namespace sbus_bridge +{ + +SBusBridge::SBusBridge(const rclcpp::Node::SharedPtr &node) + : node_(node), + logger_(node->get_logger()), + stop_watchdog_thread_(false), + time_last_rc_msg_received_(), + time_last_sbus_msg_sent_(node_->now()), + time_last_battery_voltage_received_(node_->now()), + time_last_active_control_command_received_(), + bridge_state_(BridgeState::KILL), + control_mode_(ControlMode::NONE), + arming_counter_(0), + battery_voltage_(0.0), + bridge_armed_(false), + rc_was_disarmed_once_(false), + destructor_invoked_(false) +{ + serial_port_ = std::make_unique(); + if(!loadParameters()) + { + RCLCPP_ERROR(logger_, "Could not load parameters."); + rclcpp::shutdown(); + return; + } + + if(!serial_port_->setUpSBusSerialPort(port_name_, node->get_clock())) + { + rclcpp::shutdown(); + return; + } + + // Start watchdog thread + try + { + watchdog_thread_ = std::thread(&SBusBridge::watchdogThread, this); + } + catch(...) + { + RCLCPP_ERROR(logger_, "Could not successfully start watchdog thread. Exiting SBusBridge."); + rclcpp::shutdown(); + return; + } + + RCLCPP_INFO(logger_, "SBusBridge initialized"); +} + +SBusBridge::~SBusBridge() +{ + destructor_invoked_ = true; + serial_port_->stopReceiverThread(); + + // Stop watchdog thread + stop_watchdog_thread_ = true; + // Wait for watchdog thread to finish + watchdog_thread_.join(); + + // Now only one thread (the ROS thread) is remaining + + setBridgeState(BridgeState::OFF); + + // Send disarming SBus message for safety + // We repeat it to prevent any possible smoothing of commands on the flight + // controller to interfere with this + SBusMsg shut_down_message; + shut_down_message.setArmStateDisarmed(); + rclcpp::Rate loop_rate(110.0); + for(int i = 0; i < kSmoothingFailRepetitions_; i++) + { + serial_port_->transmitSerialSBusMessage(shut_down_message); + loop_rate.sleep(); + } + + // Close serial port + serial_port_->disconnectSerialPort(); +} + +void SBusBridge::watchdogThread() +{ + rclcpp::Rate watchdog_rate(110.0); + while(rclcpp::ok() && !stop_watchdog_thread_) + { + watchdog_rate.sleep(); + + std::lock_guard main_lock(main_mutex_); + + const rclcpp::Time time_now = node_->now(); + + if(bridge_state_ == BridgeState::RC_FLIGHT && (time_now - time_last_rc_msg_received_).seconds() > rc_timeout_) + { + // If the last received RC command was armed but was received longer than + // rc_timeout ago we switch the bridge state to AUTONOMOUS_FLIGHT. + // In case there are no valid control commands the bridge state is set to + // OFF in the next check below + RCLCPP_WARN(logger_, + "Remote control was active but no message from it was received " + "within timeout (%f s).", + rc_timeout_); + setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); + } + + // if (bridge_state_ == BridgeState::ARMING || + // bridge_state_ == BridgeState::AUTONOMOUS_FLIGHT) { + // if ((time_now - time_last_active_control_command_received_).seconds() > + // control_command_timeout_) { + // // When switching the bridge state to off, our watchdog ensures that a + // // disarming off message is repeated. + // RCLCPP_INFO(node_->get_logger(), "No active control command received, setting bridge state to OFF"); + // setBridgeState(BridgeState::OFF); + // // Note: Control could theoretically still be taken over by RC but if + // // this happened in flight it might require super human reaction since + // // in this case the quad can not be armed with non zero throttle by + // // the remote. + // } + // } + + if(bridge_state_ == BridgeState::KILL) + { + // Send off message that disarms the vehicle + // We repeat it to prevent any weird behavior that occurs if the flight + // controller is not receiving commands for a while + SBusMsg kill_msg; + kill_msg.setArmStateDisarmed(); + sendSBusMessageToSerialPort(kill_msg); + // disarm bridge if needed + if(bridge_armed_) + { + disarmBridge(); + } + } + + // Check battery voltage timeout + std::lock_guard battery_lock(battery_voltage_mutex_); + if((time_now - time_last_battery_voltage_received_).seconds() > kBatteryVoltageTimeout_) + { + battery_voltage_ = 0.0; + if(perform_thrust_voltage_compensation_) + { + RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 1000, + "Can not perform battery voltage compensation because there " + "is no recent battery voltage measurement"); + } + } + + // Mutexes are unlocked because they go out of scope here + } +} + +void SBusBridge::handleReceivedSbusMessage(const SBusMsg &received_sbus_msg) +{ + { + std::lock_guard main_lock(main_mutex_); + + time_last_rc_msg_received_ = node_->now(); + + if(received_sbus_msg.isKillSwitch()) + { + if(bridge_state_ != BridgeState::KILL) + { + setBridgeState(BridgeState::KILL); + RCLCPP_INFO(logger_, "Kill switch ON"); + } + } + else + { + if(bridge_state_ == BridgeState::KILL) + { + setBridgeState(BridgeState::OFF); + RCLCPP_WARN(logger_, "Kill switch OFF"); + } + } + + if(bridge_state_ == BridgeState::KILL) + { + // send disarm to FC to kill motors + // sendSBusMessageToSerialPort(received_sbus_msg); + + return; + } + + if(received_sbus_msg.isArmed()) + { + if(!rc_was_disarmed_once_) + { + // This flag prevents that the vehicle can be armed if the RC is armed + // on startup of the bridge + RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 1.0, + "RC needs to be disarmed once before it can take over control"); + return; + } + + // Immediately go into RC_FLIGHT state since RC always has priority + if(bridge_state_ != BridgeState::RC_FLIGHT) + { + setBridgeState(BridgeState::RC_FLIGHT); + RCLCPP_INFO(logger_, "Control authority taken over by remote control."); + } + sendSBusMessageToSerialPort(received_sbus_msg); + control_mode_ = received_sbus_msg.getControlMode(); + } + else if(bridge_state_ == BridgeState::RC_FLIGHT) + { + // If the bridge was in state RC_FLIGHT and the RC is disarmed we set the + // state to AUTONOMOUS_FLIGHT + // In case there are valid control commands, the bridge will stay in + // AUTONOMOUS_FLIGHT, otherwise the watchdog will set the state to OFF + RCLCPP_INFO(logger_, "Control authority returned by remote control."); + if(bridge_armed_) + { + RCLCPP_INFO(logger_, "Bridge armed, setting bridge state to AUTONOMOUS_FLIGHT"); + setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); + } + else + { + // When switching the bridge state to off, our watchdog ensures that a + // disarming off message is sent + RCLCPP_INFO(logger_, "Bridge not armed, setting bridge state to OFF"); + setBridgeState(BridgeState::OFF); + } + } + else if(!rc_was_disarmed_once_) + { + RCLCPP_INFO(logger_, "Received disarmed RC message but RC was not disarmed once, ignoring it"); + rc_was_disarmed_once_ = true; + } + else if(bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) + { + // not killed, not armed, not autonomous flight + sendSBusMessageToSerialPort(received_sbus_msg); + } + + // Main mutex is unlocked here because it goes out of scope + } + + // received_sbus_msg_pub_.publish(received_sbus_msg.toRosMessage()); +} + +void SBusBridge::controlCommandCallback(const kr_mav_msgs::msg::SO3Command::ConstPtr &msg, + const Eigen::Quaterniond &odom_q) +{ + std::lock_guard main_lock(main_mutex_); + + if(destructor_invoked_) + { + // This ensures that if the destructor was invoked we do not try to write + // to the serial port anymore because of receiving a control command + return; + } + + // may need more logic @TODO + time_last_active_control_command_received_ = node_->now(); + + if(!bridge_armed_ || bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) + { + // If bridge is not armed we do not allow control commands to be sent + // RC has priority over control commands for autonomous flying + if(!bridge_armed_ && msg->aux.enable_motors && bridge_state_ != BridgeState::RC_FLIGHT) + { + RCLCPP_WARN(logger_, "Received active control command but sbus bridge is not armed. " + "Please arm the bridge before sending control commands."); + } + return; + } + + SBusMsg sbus_msg_to_send; + { + std::lock_guard battery_lock(battery_voltage_mutex_); + // Set commands + sbus_msg_to_send = generateSBusMessageFromSO3Command(msg, odom_q); + // Battery voltage mutex is unlocked because it goes out of scope here + } + + // Send disarm if necessary + if(!msg->aux.enable_motors) + { + if(bridge_state_ == BridgeState::ARMING || bridge_state_ == BridgeState::AUTONOMOUS_FLIGHT) + { + RCLCPP_INFO(logger_, "Control command received, setting bridge state to OFF"); + setBridgeState(BridgeState::OFF); + } + // Make sure vehicle is disarmed to immediately switch it off + sbus_msg_to_send.setArmStateDisarmed(); + } + + // Limit channels + sbus_msg_to_send.limitAllChannelsFeasible(); + + // Immediately send SBus message + sendSBusMessageToSerialPort(sbus_msg_to_send); + + // Main mutex is unlocked because it goes out of scope here +} + +void SBusBridge::sendSBusMessageToSerialPort(const SBusMsg &sbus_msg) +{ + // // print bridge state + // switch (bridge_state_) { + // case BridgeState::OFF: + // ROS_INFO("Bridge state: OFF"); + // break; + // case BridgeState::ARMING: + // ROS_INFO("Bridge state: ARMING"); + // break; + // case BridgeState::AUTONOMOUS_FLIGHT: + // ROS_INFO("Bridge state: AUTONOMOUS_FLIGHT"); + // break; + // case BridgeState::RC_FLIGHT: + // ROS_INFO("Bridge state: RC_FLIGHT"); + // break; + // default: + // ROS_INFO("Bridge state: UNKNOWN"); + // } + SBusMsg sbus_message_to_send = sbus_msg; + + switch(bridge_state_) + { + case BridgeState::OFF: + // Disarm vehicle + sbus_message_to_send.setArmStateDisarmed(); + // ROS_INFO("Bridge state: OFF"); + break; + + case BridgeState::ARMING: + // Since there might be some RC commands smoothing and checks on multiple + // messages before arming, we repeat the messages with minimum throttle + // and arming command multiple times. 5 times seems to work robustly. + if(arming_counter_ >= kSmoothingFailRepetitions_) + { + setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); + } + else + { + // Set thrust to minimum command to make sure FC arms + sbus_message_to_send.setThrottleCommand(SBusMsg::kMinCmd); + sbus_message_to_send.setArmStateArmed(); + arming_counter_++; + } + break; + + case BridgeState::AUTONOMOUS_FLIGHT: + sbus_message_to_send.setArmStateArmed(); + if(use_body_rates_) + { + sbus_message_to_send.setControlModeBodyRates(); + } + else + { + sbus_message_to_send.setControlModeAttitude(); + } + break; + + case BridgeState::RC_FLIGHT: + // Passing RC command straight through + // ROS_INFO("RC MODE: throttle_cmd %d", sbus_message_to_send.channels[sbus_bridge::channel_mapping::kThrottle]); + sbus_message_to_send.setControlModeAttitude(); + break; + + case BridgeState::KILL: + // Disarm vehicle + sbus_message_to_send.setArmStateDisarmed(); + // ROS_INFO("Bridge state: OFF"); + break; + + default: + // Disarm the vehicle because this code must be terribly wrong + sbus_message_to_send.setArmStateDisarmed(); + RCLCPP_WARN(logger_, "Bridge is in unknown state, vehicle will be disarmed"); + break; + } + + if((node_->now() - time_last_sbus_msg_sent_).seconds() <= 0.006) + { + // An SBUS message takes 3ms to be transmitted by the serial port so let's + // not stress it too much. This should only happen in case of switching + // between control commands and rc commands + if(bridge_state_ == BridgeState::ARMING) + { + // In case of arming we want to send kSmoothingFailRepetitions_ messages + // with minimum throttle to the flight controller. Since this check + // prevents the message from being sent out we reduce the counter that + // was incremented above assuming the message would actually be sent. + arming_counter_--; + } + return; + } + + sbus_message_to_send.timestamp = node_->now(); + serial_port_->transmitSerialSBusMessage(sbus_message_to_send); + time_last_sbus_msg_sent_ = node_->now(); +} + +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)); +} + +double SBusBridge::thrust_model_kartik(double thrust) const +{ + int num_props = 4; + double avg_thrust = std::max(0.0, thrust) / num_props; + + // Scale thrust to individual rotor velocities (RPM) + 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 betaflight + double throttle = (omega_avg - rpm_vs_throttle_linear_coeff_b_) / rpm_vs_throttle_linear_coeff_a_; + return throttle; +} + +SBusMsg SBusBridge::generateSBusMessageFromSO3Command(const kr_mav_msgs::msg::SO3Command::ConstPtr &msg, + const Eigen::Quaterniond &odom_q) const +{ + SBusMsg sbus_msg; + + // set sbus_msg to not killed + sbus_msg.channels[sbus_bridge::sbus_channel_mapping::kKillSwitch] = 1792; + + sbus_msg.setArmStateArmed(); + + // 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); + // const Eigen::Vector3d ang_vel(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); + + // convert to tf::Quaternion + // tf::Quaternion imu_tf = tf::Quaternion(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w()); + // tf::Quaternion odom_tf = tf::Quaternion(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w()); + + const Eigen::Matrix3d R_cur(odom_q); + + double thrust = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); + + double throttle = 0.0; + if(thrust > 1e-5) + { + throttle = thrust_model_kartik(thrust); + } + + // remap throttle (1000 to 2000) to sbus (kMinCmd to kMaxCmd) + uint16_t throttle_cmd = round(((throttle - 1000) / 1000) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); + RCLCPP_INFO_THROTTLE(logger_, *node_->get_clock(), 1.0, "AUTONOMOUS MODE: thrust: %f throttle: %f throttle_cmd: %d", + thrust, throttle, throttle_cmd); + sbus_msg.setThrottleCommand(throttle_cmd); + + // convert quaternion to euler + Eigen::Vector3d desired_euler_angles = q_des.toRotationMatrix().eulerAngles(0, 1, 2); + + // sanity check if larger than 3 rad, set to 0 + for(int i = 0; i < 3; i++) + { + if(desired_euler_angles(i) > 3.0 || desired_euler_angles(i) < -3.0) + { + desired_euler_angles(i) = 0; + } + } + + // parse commands + uint16_t roll_cmd, pitch_cmd, yaw_cmd; + + if(use_body_rates_) + { + // get body rates + double roll_rate = msg->angular_velocity.x; + double pitch_rate = msg->angular_velocity.y; + + roll_rate = std::max(-max_roll_rate_, std::min(max_roll_rate_, roll_rate)); + pitch_rate = std::max(-max_pitch_rate_, std::min(max_pitch_rate_, pitch_rate)); + + // additional safety check + roll_rate = std::max(-400.0, std::min(400.0, roll_rate)); + pitch_rate = std::max(-400.0, std::min(400.0, pitch_rate)); + + roll_cmd = round((roll_rate + max_roll_rate_) / (2 * max_roll_rate_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + + SBusMsg::kMinCmd); + pitch_cmd = round((pitch_rate + max_pitch_rate_) / (2 * max_pitch_rate_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + + SBusMsg::kMinCmd); + + RCLCPP_INFO_THROTTLE(logger_, *node_->get_clock(), 1000, + "AUTONOMOUS MODE: roll_rate: %f pitch_rate: %f roll_cmd: %d pitch_cmd: %d", roll_rate, + pitch_rate, roll_cmd, pitch_cmd); + } + else + { + // get body angles + double yaw, roll, pitch; + // use tf2 to convert quaternion to euler + tf2::Matrix3x3(tf2::Quaternion(q_des.x(), q_des.y(), q_des.z(), q_des.w())).getRPY(roll, pitch, yaw); + // convert to degrees + double roll_deg = roll * 180.0 / M_PI; + double pitch_deg = pitch * 180.0 / M_PI; + + // clip max roll + roll_deg = std::max(-max_roll_angle_, std::min(max_roll_angle_, roll_deg)); + // clip max pitch + pitch_deg = std::max(-max_pitch_angle_, std::min(max_pitch_angle_, pitch_deg)); + + roll_cmd = round((roll_deg + max_roll_angle_) / (2 * max_roll_angle_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + + SBusMsg::kMinCmd); + pitch_cmd = round((pitch_deg + max_pitch_angle_) / (2 * max_pitch_angle_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + + SBusMsg::kMinCmd); + + RCLCPP_INFO_THROTTLE(logger_, *node_->get_clock(), 1000, + "AUTONOMOUS MODE: roll_deg: %f pitch_deg: %f roll_cmd: %d pitch_cmd: %d", roll_deg, pitch_deg, + roll_cmd, pitch_cmd); + } + + // always use yaw rate + double yaw_rate = -msg->angular_velocity.z; // flip sign since FC yaw is inverted (z-down) + yaw_rate = yaw_rate * 180.0 / M_PI; // convert to degrees + yaw_rate = std::max(-400.0, std::min(400.0, yaw_rate)); // clip max yaw rate + yaw_cmd = round((yaw_rate + max_yaw_rate_) / (2 * max_yaw_rate_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + + SBusMsg::kMinCmd); + + // RCLCPP_INFO_THROTTLE( + // logger_, *node_->get_clock(), 1.0, + // "AUTONOMOUS MODE: yaw_rate: %f yaw_cmd: %d", + // yaw_rate, yaw_cmd); + + sbus_msg.setRollCommand(roll_cmd); + sbus_msg.setPitchCommand(pitch_cmd); + sbus_msg.setYawCommand(yaw_cmd); + + return sbus_msg; +} + +void SBusBridge::setBridgeState(const BridgeState &desired_bridge_state) +{ + switch(desired_bridge_state) + { + case BridgeState::OFF: + bridge_state_ = desired_bridge_state; + break; + + case BridgeState::ARMING: + bridge_state_ = desired_bridge_state; + arming_counter_ = 0; + break; + + case BridgeState::AUTONOMOUS_FLIGHT: + bridge_state_ = desired_bridge_state; + break; + + case BridgeState::RC_FLIGHT: + bridge_state_ = desired_bridge_state; + break; + + case BridgeState::KILL: + bridge_state_ = desired_bridge_state; + break; + + default: + RCLCPP_WARN(logger_, "Wanted to switch to unknown bridge state, setting to OFF"); + bridge_state_ = BridgeState::OFF; + } +} + +void SBusBridge::armBridge() +{ + std::lock_guard main_lock(arm_mutex_); + bridge_armed_ = true; +} + +void SBusBridge::disarmBridge() +{ + std::lock_guard main_lock(arm_mutex_); + bridge_armed_ = false; +} + +bool SBusBridge::isBridgeArmed() const +{ + return bridge_armed_; +} + +bool SBusBridge::loadParameters() +{ + node_->declare_parameter("port_name", std::string("/dev/ttyUSB0")); + node_->declare_parameter("control_command_timeout", 0.5); + node_->declare_parameter("rc_timeout", 0.5); + node_->declare_parameter("mass", 1.0); + node_->declare_parameter("disable_thrust_mapping", false); + node_->declare_parameter("max_roll_rate", 400.0); + node_->declare_parameter("max_pitch_rate", 400.0); + node_->declare_parameter("max_yaw_rate", 400.0); + node_->declare_parameter("max_roll_angle", 0.5); + node_->declare_parameter("max_pitch_angle", 0.5); + node_->declare_parameter("alpha_vbat_filter", 0.1); + node_->declare_parameter("perform_thrust_voltage_compensation", true); + node_->declare_parameter("n_lipo_cells", 3); + node_->declare_parameter("thrust_vs_rpm_cof_a_", 0.0001); + node_->declare_parameter("thrust_vs_rpm_cof_b_", 0.0); + node_->declare_parameter("thrust_vs_rpm_cof_c_", 0.0); + node_->declare_parameter("rpm_vs_throttle_linear_coeff_a_", 0.0001); + node_->declare_parameter("rpm_vs_throttle_linear_coeff_b_", 0.0); + node_->declare_parameter("rpm_vs_throttle_quadratic_coeff_a_", 0.0001); + node_->declare_parameter("rpm_vs_throttle_quadratic_coeff_b_", 0.0); + node_->declare_parameter("rpm_vs_throttle_quadratic_coeff_c_", 0.0); + node_->declare_parameter("use_body_rates", false); + + // Get parameters + if(!node_->get_parameter("port_name", port_name_) || + !node_->get_parameter("control_command_timeout", control_command_timeout_) || + !node_->get_parameter("rc_timeout", rc_timeout_) || !node_->get_parameter("mass", mass_) || + !node_->get_parameter("disable_thrust_mapping", disable_thrust_mapping_) || + !node_->get_parameter("max_roll_rate", max_roll_rate_) || + !node_->get_parameter("max_pitch_rate", max_pitch_rate_) || !node_->get_parameter("max_yaw_rate", max_yaw_rate_) || + !node_->get_parameter("max_roll_angle", max_roll_angle_) || + !node_->get_parameter("max_pitch_angle", max_pitch_angle_) || + !node_->get_parameter("alpha_vbat_filter", alpha_vbat_filter_) || + !node_->get_parameter("perform_thrust_voltage_compensation", perform_thrust_voltage_compensation_) || + !node_->get_parameter("n_lipo_cells", n_lipo_cells_) || + !node_->get_parameter("thrust_vs_rpm_cof_a_", thrust_vs_rpm_cof_a_) || + !node_->get_parameter("thrust_vs_rpm_cof_b_", thrust_vs_rpm_cof_b_) || + !node_->get_parameter("thrust_vs_rpm_cof_c_", thrust_vs_rpm_cof_c_) || + !node_->get_parameter("rpm_vs_throttle_linear_coeff_a_", rpm_vs_throttle_linear_coeff_a_) || + !node_->get_parameter("rpm_vs_throttle_linear_coeff_b_", rpm_vs_throttle_linear_coeff_b_) || + !node_->get_parameter("rpm_vs_throttle_quadratic_coeff_a_", rpm_vs_throttle_quadratic_coeff_a_) || + !node_->get_parameter("rpm_vs_throttle_quadratic_coeff_b_", rpm_vs_throttle_quadratic_coeff_b_) || + !node_->get_parameter("rpm_vs_throttle_quadratic_coeff_c_", rpm_vs_throttle_quadratic_coeff_c_) || + !node_->get_parameter("use_body_rates", use_body_rates_)) + { + RCLCPP_ERROR(logger_, "Failed to load one or more parameters."); + return false; + } + return true; +} + +} // namespace sbus_bridge diff --git a/interfaces/kr_betaflight_interface/src/sbus/sbus_msg.cpp b/interfaces/kr_betaflight_interface/src/sbus/sbus_msg.cpp new file mode 100644 index 00000000..845a08ac --- /dev/null +++ b/interfaces/kr_betaflight_interface/src/sbus/sbus_msg.cpp @@ -0,0 +1,144 @@ +#include "kr_betaflight_interface/sbus/sbus_msg.h" +#include "kr_betaflight_interface/sbus/sbus_channel_mapping.h" + +namespace sbus_bridge +{ + +SBusMsg::SBusMsg() + : timestamp(rclcpp::Clock().now()), + digital_channel_1(false), + digital_channel_2(false), + frame_lost(false), + failsafe(false) +{ + for(int i = 0; i < kNChannels; i++) + { + channels[i] = kMeanCmd; + } +} + +SBusMsg::~SBusMsg() {} + +void SBusMsg::limitAllChannelsFeasible() +{ + for(uint8_t i = 0; i < kNChannels; i++) + { + limitSbusChannelFeasible(i); + } +} + +void SBusMsg::limitSbusChannelFeasible(const int channel_idx) +{ + if(channel_idx < 0 || channel_idx >= kNChannels) + { + return; + } + + if(channels[channel_idx] > kMaxCmd) + { + channels[channel_idx] = kMaxCmd; + } + if(channels[channel_idx] < kMinCmd) + { + channels[channel_idx] = kMinCmd; + } +} + +void SBusMsg::setThrottleCommand(const uint16_t throttle_cmd) +{ + channels[sbus_channel_mapping::kThrottle] = throttle_cmd; +} + +void SBusMsg::setRollCommand(const uint16_t roll_cmd) +{ + channels[sbus_channel_mapping::kRoll] = roll_cmd; +} + +void SBusMsg::setPitchCommand(const uint16_t pitch_cmd) +{ + channels[sbus_channel_mapping::kPitch] = pitch_cmd; +} + +void SBusMsg::setYawCommand(const uint16_t yaw_cmd) +{ + channels[sbus_channel_mapping::kYaw] = yaw_cmd; +} + +void SBusMsg::setControlMode(const ControlMode &control_mode) +{ + if(control_mode == ControlMode::ATTITUDE) + { + channels[sbus_channel_mapping::kControlMode] = kMinCmd; + } + else if(control_mode == ControlMode::BODY_RATES) + { + channels[sbus_channel_mapping::kControlMode] = kMaxCmd; + } +} + +void SBusMsg::setControlModeAttitude() +{ + setControlMode(ControlMode::ATTITUDE); +} + +void SBusMsg::setControlModeBodyRates() +{ + setControlMode(ControlMode::BODY_RATES); +} + +void SBusMsg::setArmState(const ArmState &arm_state) +{ + if(arm_state == ArmState::ARMED) + { + channels[sbus_channel_mapping::kArming] = kMaxCmd; + } + else + { + channels[sbus_channel_mapping::kArming] = kMinCmd; + } +} + +void SBusMsg::setArmStateArmed() +{ + setArmState(ArmState::ARMED); +} + +void SBusMsg::setArmStateDisarmed() +{ + setArmState(ArmState::DISARMED); + // Should not be necessary but for safety we also set the throttle command + // to the minimum + setThrottleCommand(kMinCmd); +} + +bool SBusMsg::isArmed() const +{ + if(channels[sbus_channel_mapping::kArming] <= kMeanCmd) + { + return false; + } + + return true; +} + +bool SBusMsg::isKillSwitch() const +{ + if(channels[sbus_channel_mapping::kKillSwitch] > kMeanCmd) + { + return false; + } + + return true; +} + +ControlMode SBusMsg::getControlMode() const +{ + if(channels[sbus_channel_mapping::kControlMode] > kMeanCmd) + { + return ControlMode::BODY_RATES; + } + + return ControlMode::ATTITUDE; +} + +} // namespace sbus_bridge diff --git a/interfaces/kr_betaflight_interface/src/sbus/sbus_serial_port.cpp b/interfaces/kr_betaflight_interface/src/sbus/sbus_serial_port.cpp new file mode 100644 index 00000000..806c2eb4 --- /dev/null +++ b/interfaces/kr_betaflight_interface/src/sbus/sbus_serial_port.cpp @@ -0,0 +1,405 @@ +#include "kr_betaflight_interface/sbus/sbus_serial_port.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace sbus_bridge { + +SBusSerialPort::SBusSerialPort() + : receiver_thread_(), + receiver_thread_should_exit_(false), + serial_port_fd_(-1), + clock_(nullptr) { + logger_ = rclcpp::get_logger("sbus_serial_port"); +} + +SBusSerialPort::SBusSerialPort(const std::string& port, + const rclcpp::Clock::SharedPtr& clock) + : receiver_thread_(), + receiver_thread_should_exit_(false), + serial_port_fd_(-1), + clock_(clock) { + logger_ = rclcpp::get_logger("sbus_serial_port"); +} + +SBusSerialPort::~SBusSerialPort() { disconnectSerialPort(); } + +bool SBusSerialPort::setUpSBusSerialPort(const std::string& port, + const rclcpp::Clock::SharedPtr& clock) { + clock_ = clock; + if (!connectSerialPort(port)) { + return false; + } + + if (!startReceiverThread()) { + return false; + } + + return true; +} + +bool SBusSerialPort::connectSerialPort(const std::string& port) { + // Open serial port + // O_RDWR - Read and write + // O_NOCTTY - Ignore special chars like CTRL-C + serial_port_fd_ = open(port.c_str(), O_RDWR | O_NOCTTY); + + if (serial_port_fd_ == -1) { + RCLCPP_ERROR(logger_, "Could not open serial port %s", port.c_str()); + return false; + } + + if (!configureSerialPortForSBus()) { + close(serial_port_fd_); + RCLCPP_ERROR(logger_, "Could not set necessary configuration of serial port"); + return false; + } + + RCLCPP_INFO(logger_, "Connected to serial port %s", port.c_str()); + return true; +} + +void SBusSerialPort::disconnectSerialPort() { + stopReceiverThread(); + if (serial_port_fd_ != -1) { + close(serial_port_fd_); + serial_port_fd_ = -1; + } +} + +bool SBusSerialPort::startReceiverThread() { + // Start watchdog thread + try { + receiver_thread_ = + std::thread(&SBusSerialPort::serialPortReceiveThread, this); + } catch (...) { + RCLCPP_ERROR(logger_, "Could not successfully start SBUS receiver thread."); + return false; + } + + return true; +} + +bool SBusSerialPort::stopReceiverThread() { + if (!receiver_thread_.joinable()) { + return true; + } + + receiver_thread_should_exit_ = true; + + // Wait for receiver thread to finish + receiver_thread_.join(); + + return true; +} + +bool SBusSerialPort::configureSerialPortForSBus() const { + // clear config + fcntl(serial_port_fd_, F_SETFL, 0); + // read non blocking + fcntl(serial_port_fd_, F_SETFL, FNDELAY); + + struct termios2 uart_config; + /* Fill the struct for the new configuration */ + ioctl(serial_port_fd_, TCGETS2, &uart_config); + + // Output flags - Turn off output processing + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + + // Input flags - Turn off input processing + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= + ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + + // + // No line processing: + // echo off + // echo newline off + // canonical mode off, + // extended input processing off + // signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + // Turn off character processing + // Turn off odd parity + uart_config.c_cflag &= ~(CSIZE | PARODD | CBAUD); + + // Enable parity generation on output and parity checking for input. + uart_config.c_cflag |= PARENB; + // Set two stop bits, rather than one. + uart_config.c_cflag |= CSTOPB; + // No output processing, force 8 bit input + uart_config.c_cflag |= CS8; + // Enable a non standard baud rate + uart_config.c_cflag |= BOTHER; + + // Set custom baud rate of 100'000 bits/s necessary for sbus + const speed_t spd = 100000; + uart_config.c_ispeed = spd; + uart_config.c_ospeed = spd; + + if (ioctl(serial_port_fd_, TCSETS2, &uart_config) < 0) { + RCLCPP_ERROR(logger_, "Could not set configuration of serial port"); + return false; + } + + return true; +} + +void SBusSerialPort::transmitSerialSBusMessage(const SBusMsg& sbus_msg) const { + static uint8_t buffer[kSbusFrameLength_]; + + // SBUS header + buffer[0] = kSbusHeaderByte_; + + // 16 channels of 11 bit data + buffer[1] = (uint8_t)((sbus_msg.channels[0] & 0x07FF)); + buffer[2] = (uint8_t)((sbus_msg.channels[0] & 0x07FF) >> 8 | + (sbus_msg.channels[1] & 0x07FF) << 3); + buffer[3] = (uint8_t)((sbus_msg.channels[1] & 0x07FF) >> 5 | + (sbus_msg.channels[2] & 0x07FF) << 6); + buffer[4] = (uint8_t)((sbus_msg.channels[2] & 0x07FF) >> 2); + buffer[5] = (uint8_t)((sbus_msg.channels[2] & 0x07FF) >> 10 | + (sbus_msg.channels[3] & 0x07FF) << 1); + buffer[6] = (uint8_t)((sbus_msg.channels[3] & 0x07FF) >> 7 | + (sbus_msg.channels[4] & 0x07FF) << 4); + buffer[7] = (uint8_t)((sbus_msg.channels[4] & 0x07FF) >> 4 | + (sbus_msg.channels[5] & 0x07FF) << 7); + buffer[8] = (uint8_t)((sbus_msg.channels[5] & 0x07FF) >> 1); + buffer[9] = (uint8_t)((sbus_msg.channels[5] & 0x07FF) >> 9 | + (sbus_msg.channels[6] & 0x07FF) << 2); + buffer[10] = (uint8_t)((sbus_msg.channels[6] & 0x07FF) >> 6 | + (sbus_msg.channels[7] & 0x07FF) << 5); + buffer[11] = (uint8_t)((sbus_msg.channels[7] & 0x07FF) >> 3); + buffer[12] = (uint8_t)((sbus_msg.channels[8] & 0x07FF)); + buffer[13] = (uint8_t)((sbus_msg.channels[8] & 0x07FF) >> 8 | + (sbus_msg.channels[9] & 0x07FF) << 3); + buffer[14] = (uint8_t)((sbus_msg.channels[9] & 0x07FF) >> 5 | + (sbus_msg.channels[10] & 0x07FF) << 6); + buffer[15] = (uint8_t)((sbus_msg.channels[10] & 0x07FF) >> 2); + buffer[16] = (uint8_t)((sbus_msg.channels[10] & 0x07FF) >> 10 | + (sbus_msg.channels[11] & 0x07FF) << 1); + buffer[17] = (uint8_t)((sbus_msg.channels[11] & 0x07FF) >> 7 | + (sbus_msg.channels[12] & 0x07FF) << 4); + buffer[18] = (uint8_t)((sbus_msg.channels[12] & 0x07FF) >> 4 | + (sbus_msg.channels[13] & 0x07FF) << 7); + buffer[19] = (uint8_t)((sbus_msg.channels[13] & 0x07FF) >> 1); + buffer[20] = (uint8_t)((sbus_msg.channels[13] & 0x07FF) >> 9 | + (sbus_msg.channels[14] & 0x07FF) << 2); + buffer[21] = (uint8_t)((sbus_msg.channels[14] & 0x07FF) >> 6 | + (sbus_msg.channels[15] & 0x07FF) << 5); + buffer[22] = (uint8_t)((sbus_msg.channels[15] & 0x07FF) >> 3); + + // SBUS flags + // (bit0 = least significant bit) + // bit0 = ch17 = digital channel (0x01) + // bit1 = ch18 = digital channel (0x02) + // bit2 = Frame lost, equivalent red LED on receiver (0x04) + // bit3 = Failsafe activated (0x08) + // bit4 = n/a + // bit5 = n/a + // bit6 = n/a + // bit7 = n/a + buffer[23] = 0x00; + if (sbus_msg.digital_channel_1) { + buffer[23] |= 0x01; + } + if (sbus_msg.digital_channel_2) { + buffer[23] |= 0x02; + } + if (sbus_msg.frame_lost) { + buffer[23] |= 0x04; + } + if (sbus_msg.failsafe) { + buffer[23] |= 0x08; + } + + // SBUS footer + buffer[24] = kSbusFooterByte_; + + const int written = write(serial_port_fd_, (char*)buffer, kSbusFrameLength_); + // tcflush(serial_port_fd_, TCOFLUSH); // There were rumors that this might + // not work on Odroids... + if (written != kSbusFrameLength_) { + RCLCPP_ERROR(logger_, "Wrote %d bytes but should have written %d", written, kSbusFrameLength_); + } +} + +void SBusSerialPort::serialPortReceiveThread() { + struct pollfd fds[1]; + fds[0].fd = serial_port_fd_; + fds[0].events = POLLIN; + + uint8_t init_buf[10]; + while (read(serial_port_fd_, init_buf, sizeof(init_buf)) > 0) { + // On startup, as long as we receive something, we keep reading to ensure + // that the first byte of the first poll is the start of an SBUS message + // and not some arbitrary byte. + // This should help to get the framing in sync in the beginning. + usleep(100); + } + + std::deque bytes_buf; + + while (!receiver_thread_should_exit_) { + // Buffer to read bytes from serial port. We make it large enough to + // potentially contain 4 sbus messages but its actual size probably does + // not matter too much + uint8_t read_buf[4 * kSbusFrameLength_]; + + if (poll(fds, 1, kPollTimeoutMilliSeconds_) > 0) { + if (fds[0].revents & POLLIN) { + const ssize_t nread = read(serial_port_fd_, read_buf, sizeof(read_buf)); + + for (ssize_t i = 0; i < nread; i++) { + bytes_buf.push_back(read_buf[i]); + } + + bool valid_sbus_message_received = false; + uint8_t sbus_msg_bytes[kSbusFrameLength_]; + while (bytes_buf.size() >= kSbusFrameLength_) { + // Check if we have a potentially valid SBUS message + // A valid SBUS message must have to correct header and footer byte + // as well as zeros in the four most significant bytes of the flag + // byte (byte 23) + if (bytes_buf.front() == kSbusHeaderByte_ && + !(bytes_buf[kSbusFrameLength_ - 2] & 0xF0) && + bytes_buf[kSbusFrameLength_ - 1] == kSbusFooterByte_) { + for (uint8_t i = 0; i < kSbusFrameLength_; i++) { + sbus_msg_bytes[i] = bytes_buf.front(); + bytes_buf.pop_front(); + } + + valid_sbus_message_received = true; + } else { + // If it is not a valid SBUS message but has a correct header byte + // we need to pop it to prevent staying in this loop forever + bytes_buf.pop_front(); + RCLCPP_WARN(logger_, "SBUS message framing not in sync"); + } + + // If not, pop front elements until we have a valid header byte + while (!bytes_buf.empty() && bytes_buf.front() != kSbusHeaderByte_) { + bytes_buf.pop_front(); + } + } + + if (valid_sbus_message_received) { + // Sometimes we read more than one sbus message at the same time + // By running the loop above for as long as possible before handling + // the received sbus message we achieve to only process the latest one + const SBusMsg received_sbus_msg = parseSbusMessage(sbus_msg_bytes); + handleReceivedSbusMessage(received_sbus_msg); + } + } + } + } + + return; +} + +SBusMsg SBusSerialPort::parseSbusMessage( + uint8_t sbus_msg_bytes[kSbusFrameLength_]) const { + SBusMsg sbus_msg; + + if (clock_) { + sbus_msg.timestamp = clock_->now(); + } else { + sbus_msg.timestamp = rclcpp::Clock().now(); + } + + // Decode the 16 regular channels + sbus_msg.channels[0] = + (((uint16_t)sbus_msg_bytes[1]) | ((uint16_t)sbus_msg_bytes[2] << 8)) & + 0x07FF; + sbus_msg.channels[1] = (((uint16_t)sbus_msg_bytes[2] >> 3) | + ((uint16_t)sbus_msg_bytes[3] << 5)) & + 0x07FF; + sbus_msg.channels[2] = + (((uint16_t)sbus_msg_bytes[3] >> 6) | ((uint16_t)sbus_msg_bytes[4] << 2) | + ((uint16_t)sbus_msg_bytes[5] << 10)) & + 0x07FF; + sbus_msg.channels[3] = (((uint16_t)sbus_msg_bytes[5] >> 1) | + ((uint16_t)sbus_msg_bytes[6] << 7)) & + 0x07FF; + sbus_msg.channels[4] = (((uint16_t)sbus_msg_bytes[6] >> 4) | + ((uint16_t)sbus_msg_bytes[7] << 4)) & + 0x07FF; + sbus_msg.channels[5] = + (((uint16_t)sbus_msg_bytes[7] >> 7) | ((uint16_t)sbus_msg_bytes[8] << 1) | + ((uint16_t)sbus_msg_bytes[9] << 9)) & + 0x07FF; + sbus_msg.channels[6] = (((uint16_t)sbus_msg_bytes[9] >> 2) | + ((uint16_t)sbus_msg_bytes[10] << 6)) & + 0x07FF; + sbus_msg.channels[7] = (((uint16_t)sbus_msg_bytes[10] >> 5) | + ((uint16_t)sbus_msg_bytes[11] << 3)) & + 0x07FF; + sbus_msg.channels[8] = + (((uint16_t)sbus_msg_bytes[12]) | ((uint16_t)sbus_msg_bytes[13] << 8)) & + 0x07FF; + sbus_msg.channels[9] = (((uint16_t)sbus_msg_bytes[13] >> 3) | + ((uint16_t)sbus_msg_bytes[14] << 5)) & + 0x07FF; + sbus_msg.channels[10] = (((uint16_t)sbus_msg_bytes[14] >> 6) | + ((uint16_t)sbus_msg_bytes[15] << 2) | + ((uint16_t)sbus_msg_bytes[16] << 10)) & + 0x07FF; + sbus_msg.channels[11] = (((uint16_t)sbus_msg_bytes[16] >> 1) | + ((uint16_t)sbus_msg_bytes[17] << 7)) & + 0x07FF; + sbus_msg.channels[12] = (((uint16_t)sbus_msg_bytes[17] >> 4) | + ((uint16_t)sbus_msg_bytes[18] << 4)) & + 0x07FF; + sbus_msg.channels[13] = (((uint16_t)sbus_msg_bytes[18] >> 7) | + ((uint16_t)sbus_msg_bytes[19] << 1) | + ((uint16_t)sbus_msg_bytes[20] << 9)) & + 0x07FF; + sbus_msg.channels[14] = (((uint16_t)sbus_msg_bytes[20] >> 2) | + ((uint16_t)sbus_msg_bytes[21] << 6)) & + 0x07FF; + sbus_msg.channels[15] = (((uint16_t)sbus_msg_bytes[21] >> 5) | + ((uint16_t)sbus_msg_bytes[22] << 3)) & + 0x07FF; + + // Decode flags from byte 23 (see comments in sendSBusMessage function) + // By default, flags are set to false in SBusMsg constructor, so we only need + // to set them true if necessary + if (sbus_msg_bytes[23] & (1 << 0)) { + sbus_msg.digital_channel_1 = true; + } + + if (sbus_msg_bytes[23] & (1 << 1)) { + sbus_msg.digital_channel_2 = true; + } + + if (sbus_msg_bytes[23] & (1 << 2)) { + sbus_msg.frame_lost = true; + } + + if (sbus_msg_bytes[23] & (1 << 3)) { + sbus_msg.failsafe = true; + } + + return sbus_msg; +} + +} // namespace sbus_bridge diff --git a/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp b/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp new file mode 100644 index 00000000..b4830080 --- /dev/null +++ b/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp @@ -0,0 +1,148 @@ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +using namespace std::chrono_literals; + +class SO3CmdToBetaflight : public rclcpp::Node +{ +public: + explicit SO3CmdToBetaflight(const rclcpp::NodeOptions &options); + +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 check_so3_cmd_timeout(); + void motors_on(); + void motors_off(); + + // Controller state + bool odom_set_, imu_set_, so3_cmd_set_; + Eigen::Quaterniond odom_q_; + + // Subscribers + rclcpp::Subscription::SharedPtr so3_cmd_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + + // Motor status + int motor_status_; + + // Timeout handling + double so3_cmd_timeout_; + rclcpp::Time last_so3_cmd_time_; + kr_mav_msgs::msg::SO3Command last_so3_cmd_; + + // Protocol bridge (CRSF or SBUS) + std::shared_ptr bridge_; + rclcpp::TimerBase::SharedPtr init_timer_; +}; + +SO3CmdToBetaflight::SO3CmdToBetaflight(const rclcpp::NodeOptions &options) + : Node("so3cmd_to_bridge", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + odom_set_(false), + imu_set_(false), + so3_cmd_set_(false), + motor_status_(0) +{ + this->declare_parameter("so3_cmd_timeout", 0.25); + this->declare_parameter("protocol_type", "crsf"); + so3_cmd_timeout_ = this->get_parameter("so3_cmd_timeout").as_double(); + std::string protocol_type = this->get_parameter("protocol_type").as_string(); + + // Create subscribers + so3_cmd_sub_ = this->create_subscription( + "so3_cmd", 10, + std::bind(&SO3CmdToBetaflight::so3_cmd_callback, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + "odom", 10, + std::bind(&SO3CmdToBetaflight::odom_callback, this, std::placeholders::_1)); + + init_timer_ = this->create_wall_timer( + std::chrono::milliseconds(0), + [this, protocol_type]() { + this->init_timer_->cancel(); + if (protocol_type == "crsf") { + bridge_ = std::make_shared(shared_from_this()); + } else if (protocol_type == "sbus") { + bridge_ = std::make_shared(shared_from_this()); + } else { + RCLCPP_ERROR(this->get_logger(), "Unknown protocol_type: %s", protocol_type.c_str()); + } + } + ); + + RCLCPP_INFO(this->get_logger(), "SO3CmdToBetaflight component initialized with protocol: %s", protocol_type.c_str()); +} + +void SO3CmdToBetaflight::so3_cmd_callback(const kr_mav_msgs::msg::SO3Command::SharedPtr msg) +{ + if (!so3_cmd_set_) + so3_cmd_set_ = true; + + // Switch on motors + if (msg->aux.enable_motors && bridge_ && !bridge_->isBridgeArmed() && !motor_status_) + { + motors_on(); + } + else if (!msg->aux.enable_motors && bridge_) + { + motors_off(); + } + + if (bridge_) + bridge_->controlCommandCallback(msg, odom_q_); + + // Save last so3_cmd + last_so3_cmd_ = *msg; + last_so3_cmd_time_ = this->now(); +} + +void SO3CmdToBetaflight::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; +} + +void SO3CmdToBetaflight::check_so3_cmd_timeout() +{ + if (so3_cmd_set_ && ((this->now() - last_so3_cmd_time_).seconds() >= so3_cmd_timeout_)) + { + RCLCPP_DEBUG(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 SO3CmdToBetaflight::motors_on() +{ + if (bridge_) bridge_->armBridge(); + motor_status_ = 1; +} + +void SO3CmdToBetaflight::motors_off() +{ + if (bridge_) bridge_->disarmBridge(); + motor_status_ = 0; +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(SO3CmdToBetaflight) From b88d0c79c9bfd9ad8617f3ac584daf3c7706e6b3 Mon Sep 17 00:00:00 2001 From: Dexter Date: Mon, 15 Sep 2025 16:08:45 -0400 Subject: [PATCH 43/64] Working betaflight_interface --- .../config/neurofly.yaml | 2 +- .../crsf/crsf_serial_port.h | 6 +-- .../sbus/sbus_serial_port.h | 8 ++-- .../src/crsf/crsf_bridge.cpp | 5 +++ .../src/crsf/crsf_serial_port.cpp | 38 ++++++++++++++----- .../src/sbus/sbus_bridge.cpp | 5 +++ .../src/sbus/sbus_serial_port.cpp | 10 ++++- 7 files changed, 55 insertions(+), 19 deletions(-) diff --git a/interfaces/kr_betaflight_interface/config/neurofly.yaml b/interfaces/kr_betaflight_interface/config/neurofly.yaml index 6044b65a..8fccefb2 100644 --- a/interfaces/kr_betaflight_interface/config/neurofly.yaml +++ b/interfaces/kr_betaflight_interface/config/neurofly.yaml @@ -1,5 +1,5 @@ port_name: /dev/ttyTHS1 -enable_receiving_crsf_messages: true +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] diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_serial_port.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_serial_port.h index 1db9fd29..39fa0930 100644 --- a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_serial_port.h +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_serial_port.h @@ -25,10 +25,10 @@ class CrsfSerialPort { bool stopReceiverThread(); void transmitSerialCrsfMessage(const CrsfMsg& crsf_msg) const; - virtual void handleReceivedCrsfMessage( - const crsf_bridge::CrsfMsg& received_crsf_msg) {} + void setMessageCallback(std::function cb); - private: +private: + std::function message_callback_; static constexpr int kCrsfFrameLength_ = 24; // CRSF frame length (update as needed) static constexpr uint8_t kCrsfHeaderByte_ = 0xC8; // CRSF header (update as needed) static constexpr int kPollTimeoutMilliSeconds_ = 500; diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_serial_port.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_serial_port.h index 8ba17d77..3e682e06 100644 --- a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_serial_port.h +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_serial_port.h @@ -24,10 +24,10 @@ class SBusSerialPort { bool stopReceiverThread(); void transmitSerialSBusMessage(const SBusMsg& sbus_msg) const; - virtual void handleReceivedSbusMessage( - const sbus_bridge::SBusMsg& received_sbus_msg) {} + void setMessageCallback(std::function cb); - private: +private: + std::function message_callback_; static constexpr int kSbusFrameLength_ = 25; static constexpr uint8_t kSbusHeaderByte_ = 0x0F; static constexpr uint8_t kSbusFooterByte_ = 0x00; @@ -36,7 +36,7 @@ class SBusSerialPort { bool configureSerialPortForSBus() const; void serialPortReceiveThread(); sbus_bridge::SBusMsg parseSbusMessage( - uint8_t sbus_msg_bytes[kSbusFrameLength_]) const; + uint8_t sbus_msg_bytes[kSbusFrameLength_]) const; std::thread receiver_thread_; std::atomic_bool receiver_thread_should_exit_; diff --git a/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp b/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp index 1a4737ab..ef6d29cc 100644 --- a/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp +++ b/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp @@ -33,6 +33,11 @@ CrsfBridge::CrsfBridge(const rclcpp::Node::SharedPtr& node) return; } + // Set callback for received CRSF messages + serial_port_->setMessageCallback([this](const CrsfMsg& msg) { + this->handleReceivedCrsfMessage(msg); + }); + try { watchdog_thread_ = std::thread(&CrsfBridge::watchdogThread, this); } catch (...) { diff --git a/interfaces/kr_betaflight_interface/src/crsf/crsf_serial_port.cpp b/interfaces/kr_betaflight_interface/src/crsf/crsf_serial_port.cpp index cc5e82ad..552fe3c4 100644 --- a/interfaces/kr_betaflight_interface/src/crsf/crsf_serial_port.cpp +++ b/interfaces/kr_betaflight_interface/src/crsf/crsf_serial_port.cpp @@ -207,6 +207,17 @@ void CrsfSerialPort::serialPortReceiveThread() { for (ssize_t i = 0; i < nread; ++i) { bytes_buf.push_back(read_buf[i]); } + + // RCLCPP_INFO(logger_, "Read %zd bytes from CRSF serial port", nread); + + // // print buffer contents for debugging + // std::string buf_str; + // for (const auto& byte : bytes_buf) { + // char byte_str[4]; + // snprintf(byte_str, sizeof(byte_str), "%02X ", byte); + // buf_str += byte_str; + // } + // RCLCPP_INFO(logger_, "Buffer contents: %s", buf_str.c_str()); // Try to extract CRSF frames with proper synchronization while (bytes_buf.size() >= 3) { // Need at least address + length + type @@ -214,11 +225,13 @@ void CrsfSerialPort::serialPortReceiveThread() { if (bytes_buf[0] == 0xC8 || bytes_buf[0] == 0xEE) { uint8_t frame_length = bytes_buf[1]; uint8_t total_frame_size = frame_length + 2; // +2 for address and length bytes - + // Validate frame length and ensure we have the complete frame - if (frame_length >= 2 && frame_length <= 64 && // Reasonable length bounds - bytes_buf.size() >= total_frame_size) { // Wait for complete frame - + if (frame_length >= 2 && frame_length <= 64) { + if (bytes_buf.size() < total_frame_size) { + // Not enough data yet, break and wait for next read + break; + } // Check if this is an RC channels frame if (bytes_buf[2] == CRSF_TYPE_RC_CHANNELS_PACKED) { // Extract the complete frame @@ -234,10 +247,10 @@ void CrsfSerialPort::serialPortReceiveThread() { CrsfMsg msg = parseCrsfMessage(&frame[3]); // Skip address, length, type if (clock_) msg.timestamp = clock_->now(); else msg.timestamp = rclcpp::Clock().now(); - - RCLCPP_DEBUG(logger_, "Valid CRSF frame - Address: %02X, Length: %d, Type: %02X", - frame[0], frame[1], frame[2]); - handleReceivedCrsfMessage(msg); + + if (message_callback_) { + message_callback_(msg); + } } else { // CRC mismatch - log and discard RCLCPP_WARN(logger_, "CRSF frame CRC mismatch - discarding frame"); @@ -256,8 +269,9 @@ void CrsfSerialPort::serialPortReceiveThread() { continue; } } else { - // Incomplete frame - wait for more data - break; + // Invalid length, pop header and continue searching + bytes_buf.pop_front(); + continue; } } // Not a valid frame start, pop one byte and continue searching @@ -274,6 +288,10 @@ void CrsfSerialPort::serialPortReceiveThread() { } } +void CrsfSerialPort::setMessageCallback(std::function cb) { + message_callback_ = cb; +} + CrsfMsg CrsfSerialPort::parseCrsfMessage(const uint8_t* payload) const { CrsfMsg msg; const unsigned numOfChannels = 16; diff --git a/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp b/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp index fb8222c8..062ad5cc 100644 --- a/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp +++ b/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp @@ -36,6 +36,11 @@ SBusBridge::SBusBridge(const rclcpp::Node::SharedPtr &node) return; } + // Set callback for received SBUS messages + serial_port_->setMessageCallback([this](const SBusMsg& msg) { + this->handleReceivedSbusMessage(msg); + }); + // Start watchdog thread try { diff --git a/interfaces/kr_betaflight_interface/src/sbus/sbus_serial_port.cpp b/interfaces/kr_betaflight_interface/src/sbus/sbus_serial_port.cpp index 806c2eb4..1e423a87 100644 --- a/interfaces/kr_betaflight_interface/src/sbus/sbus_serial_port.cpp +++ b/interfaces/kr_betaflight_interface/src/sbus/sbus_serial_port.cpp @@ -1,4 +1,5 @@ #include "kr_betaflight_interface/sbus/sbus_serial_port.h" +#include #include #include @@ -13,6 +14,11 @@ namespace sbus_bridge { +// Add callback setter implementation +void SBusSerialPort::setMessageCallback(std::function cb) { + message_callback_ = cb; +} + SBusSerialPort::SBusSerialPort() : receiver_thread_(), receiver_thread_should_exit_(false), @@ -307,7 +313,9 @@ void SBusSerialPort::serialPortReceiveThread() { // By running the loop above for as long as possible before handling // the received sbus message we achieve to only process the latest one const SBusMsg received_sbus_msg = parseSbusMessage(sbus_msg_bytes); - handleReceivedSbusMessage(received_sbus_msg); + if (message_callback_) { + message_callback_(received_sbus_msg); + } } } } From e09f44684e0aa19e37163b0e08fa43946c0e46eb Mon Sep 17 00:00:00 2001 From: Dexter Date: Fri, 10 Oct 2025 16:05:50 +0000 Subject: [PATCH 44/64] Removed sbus_interface --- interfaces/kr_sbus_interface/CMakeLists.txt | 63 -- .../kr_sbus_interface/config/gains.yaml | 16 - .../config/mav_manager_params.yaml | 6 - .../kr_sbus_interface/config/neurofly.yaml | 32 - .../config/tracker_params.yaml | 25 - .../kr_sbus_interface/config/trackers.yaml | 11 - .../kr_sbus_interface/channel_mapping.h | 18 - .../include/kr_sbus_interface/sbus_bridge.h | 149 ---- .../include/kr_sbus_interface/sbus_msg.h | 61 -- .../kr_sbus_interface/sbus_serial_port.h | 52 -- .../kr_sbus_interface/thrust_mapping.h | 39 -- .../launch/control.launch.py | 213 ------ .../launch/sbus_interface.launch.py | 57 -- .../kr_sbus_interface/nodelet_plugin.xml | 7 - interfaces/kr_sbus_interface/package.xml | 23 - .../scripts/mavlink_telemetry.py | 35 - .../kr_sbus_interface/src/sbus_bridge.cpp | 663 ------------------ interfaces/kr_sbus_interface/src/sbus_msg.cpp | 112 --- .../src/sbus_serial_port.cpp | 409 ----------- .../src/so3cmd_to_sbus_component.cpp | 156 ----- .../kr_sbus_interface/src/thrust_mapping.cpp | 85 --- 21 files changed, 2232 deletions(-) delete mode 100644 interfaces/kr_sbus_interface/CMakeLists.txt delete mode 100644 interfaces/kr_sbus_interface/config/gains.yaml delete mode 100644 interfaces/kr_sbus_interface/config/mav_manager_params.yaml delete mode 100644 interfaces/kr_sbus_interface/config/neurofly.yaml delete mode 100644 interfaces/kr_sbus_interface/config/tracker_params.yaml delete mode 100644 interfaces/kr_sbus_interface/config/trackers.yaml delete mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h delete mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h delete mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h delete mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h delete mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/thrust_mapping.h delete mode 100644 interfaces/kr_sbus_interface/launch/control.launch.py delete mode 100644 interfaces/kr_sbus_interface/launch/sbus_interface.launch.py delete mode 100644 interfaces/kr_sbus_interface/nodelet_plugin.xml delete mode 100644 interfaces/kr_sbus_interface/package.xml delete mode 100644 interfaces/kr_sbus_interface/scripts/mavlink_telemetry.py delete mode 100644 interfaces/kr_sbus_interface/src/sbus_bridge.cpp delete mode 100644 interfaces/kr_sbus_interface/src/sbus_msg.cpp delete mode 100644 interfaces/kr_sbus_interface/src/sbus_serial_port.cpp delete mode 100644 interfaces/kr_sbus_interface/src/so3cmd_to_sbus_component.cpp delete mode 100644 interfaces/kr_sbus_interface/src/thrust_mapping.cpp diff --git a/interfaces/kr_sbus_interface/CMakeLists.txt b/interfaces/kr_sbus_interface/CMakeLists.txt deleted file mode 100644 index d3096411..00000000 --- a/interfaces/kr_sbus_interface/CMakeLists.txt +++ /dev/null @@ -1,63 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(kr_sbus_interface) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -include_directories( - include - ${CMAKE_CURRENT_SOURCE_DIR}/include -) - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(kr_mav_msgs REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(tf2 REQUIRED) - -add_library(${PROJECT_NAME} SHARED src/so3cmd_to_sbus_component.cpp - src/sbus_bridge.cpp - src/sbus_msg.cpp - src/sbus_serial_port.cpp) -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_components - kr_mav_msgs - nav_msgs - sensor_msgs - geometry_msgs - tf2) -target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) -rclcpp_components_register_nodes(${PROJECT_NAME} "SO3CmdToSBUS") - -# Install headers -install( - DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(TARGETS - ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) - -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/interfaces/kr_sbus_interface/config/gains.yaml b/interfaces/kr_sbus_interface/config/gains.yaml deleted file mode 100644 index 7d5eb78e..00000000 --- a/interfaces/kr_sbus_interface/config/gains.yaml +++ /dev/null @@ -1,16 +0,0 @@ -neurofly1/so3_controller: - gains: - pos: { x: 6.0, y: 6.0, z: 8.0 } - vel: { x: 3.0, y: 3.0, z: 3.5 } - ki: { x: 5.00, y: 5.00, z: 5.00 } - kib: { x: 0.00, y: 0.00, z: 0.00 } - rot: { x: 1.2, y: 1.2, z: 1.0 } - ang: { x: 0.13, y: 0.13, z: 0.1} - - corrections: - kf: 0.0e-08 - r: 0.0 - p: 0.0 - - max_pos_int: 0.3 - max_pos_int_b: 0.3 \ No newline at end of file diff --git a/interfaces/kr_sbus_interface/config/mav_manager_params.yaml b/interfaces/kr_sbus_interface/config/mav_manager_params.yaml deleted file mode 100644 index c0c3c823..00000000 --- a/interfaces/kr_sbus_interface/config/mav_manager_params.yaml +++ /dev/null @@ -1,6 +0,0 @@ -server_wait_timeout: 0.5 -need_imu: false -need_output_data: true -use_attitude_safety_catch: true -max_attitude_angle: 0.43 -takeoff_height: 0.2 diff --git a/interfaces/kr_sbus_interface/config/neurofly.yaml b/interfaces/kr_sbus_interface/config/neurofly.yaml deleted file mode 100644 index 4c40a9a5..00000000 --- a/interfaces/kr_sbus_interface/config/neurofly.yaml +++ /dev/null @@ -1,32 +0,0 @@ -port_name: /dev/ttyTHS1 -enable_receiving_sbus_messages: true -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.66 # [kg] -disable_thrust_mapping: true -use_body_rates: false -# 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 -# when flying in angle mode -max_roll_rate: 1000.0 # [deg/s] -max_pitch_rate: 1000.0 # [deg/s] -max_yaw_rate: 1000.0 # [deg/s] -max_roll_angle: 50.0 # [deg] -max_pitch_angle: 50.0 # [deg] -alpha_vbat_filter: 0.01 -perform_thrust_voltage_compensation: false -thrust_ratio_voltage_map_a: -0.17044342 # [1/V] -thrust_ratio_voltage_map_b: 3.10495276 # [-] -n_lipo_cells: 3 # [-] diff --git a/interfaces/kr_sbus_interface/config/tracker_params.yaml b/interfaces/kr_sbus_interface/config/tracker_params.yaml deleted file mode 100644 index fd97a0b0..00000000 --- a/interfaces/kr_sbus_interface/config/tracker_params.yaml +++ /dev/null @@ -1,25 +0,0 @@ -# This should contain tracker parameters - -line_tracker_distance: - default_v_des: 1.0 - default_a_des: 0.5 - epsilon: 0.1 - -line_tracker_min_jerk: - default_v_des: 1.0 - default_a_des: 0.5 - default_yaw_v_des: 0.8 - default_yaw_a_des: 0.2 - -trajectory_tracker: - max_vel_des: 0.5 - max_acc_des: 0.3 - -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_sbus_interface/config/trackers.yaml b/interfaces/kr_sbus_interface/config/trackers.yaml deleted file mode 100644 index 694f6177..00000000 --- a/interfaces/kr_sbus_interface/config/trackers.yaml +++ /dev/null @@ -1,11 +0,0 @@ -trackers: - - kr_trackers/LineTrackerMinJerk - - kr_trackers/LineTrackerDistance -# - kr_trackers/LineTrackerTrapezoid -# - kr_trackers/LineTrackerYaw - - kr_trackers/VelocityTrackerAction - - kr_trackers/NullTracker - - kr_trackers/CircleTracker - - kr_trackers/TrajectoryTracker - - kr_trackers/SmoothVelTracker - - kr_trackers/LissajousTracker diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h deleted file mode 100644 index e0e5da02..00000000 --- a/interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h +++ /dev/null @@ -1,18 +0,0 @@ -#pragma once - -namespace sbus_bridge { - -namespace channel_mapping { - -static constexpr uint8_t kThrottle = 2; -static constexpr uint8_t kRoll = 0; -static constexpr uint8_t kPitch = 1; -static constexpr uint8_t kYaw = 3; -static constexpr uint8_t kArming = 4; -static constexpr uint8_t kKillSwitch = 7; -static constexpr uint8_t kControlMode = 5; -static constexpr uint8_t kGamepadMode = 6; - -} // namespace channel_mapping - -} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h deleted file mode 100644 index b4e86b99..00000000 --- a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h +++ /dev/null @@ -1,149 +0,0 @@ -#pragma once - -#include -#include "kr_sbus_interface/sbus_msg.h" -#include "kr_sbus_interface/sbus_serial_port.h" -#include -#include -#include - -#include -#include -#include -#include - -namespace sbus_bridge -{ - -enum class BridgeState -{ - OFF, - ARMING, - AUTONOMOUS_FLIGHT, - RC_FLIGHT, - KILL -}; - -class SBusBridge : public SBusSerialPort -{ - public: - SBusBridge(const rclcpp::Node::SharedPtr &node); - - void controlCommandCallback(const kr_mav_msgs::msg::SO3Command::ConstPtr &msg, const Eigen::Quaterniond &odom_q); - void armBridge(); - void disarmBridge(); - bool isBridgeArmed() const; - - virtual ~SBusBridge(); - - private: - void watchdogThread(); - - void handleReceivedSbusMessage(const SBusMsg &received_sbus_msg) override; - - SBusMsg generateSBusMessageFromSO3Command(const kr_mav_msgs::msg::SO3Command::ConstPtr &control_command, - const Eigen::Quaterniond &odom_q) const; - - void sendSBusMessageToSerialPort(const SBusMsg &sbus_msg); - - void setBridgeState(const BridgeState &desired_bridge_state); - - double thrust_model_kartik(double thrust) const; - - bool loadParameters(); - - // rclcpp Node - rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; - - // Mutex for: - // - bridge_state_ - // - control_mode_ - // - bridge_armed_ - // - time_last_active_control_command_received_ - // - time_last_rc_msg_received_ - // - arming_counter_ - // - time_last_sbus_msg_sent_ - // Also "setBridgeState" and "sendSBusMessageToSerialPort" should only be - // called when "main_mutex_" is locked - mutable std::mutex main_mutex_; - mutable std::mutex arm_mutex_; - // Mutex for: - // - battery_voltage_ - // - time_last_battery_voltage_received_ - // Also "generateSBusMessageFromControlCommand" should only be called when - // "battery_voltage_mutex_" is locked - mutable std::mutex battery_voltage_mutex_; - - // Subscribers - rclcpp::Subscription::SharedPtr control_command_sub_; - rclcpp::Subscription::SharedPtr battery_voltage_sub_; - // ROS 2: arm_bridge_sub_ removed unless you have a custom message/type for it - - // Timer - rclcpp::TimerBase::SharedPtr low_level_feedback_pub_timer_; - - // Watchdog - std::thread watchdog_thread_; - std::atomic_bool stop_watchdog_thread_; - rclcpp::Time time_last_rc_msg_received_; - rclcpp::Time time_last_sbus_msg_sent_; - rclcpp::Time time_last_battery_voltage_received_; - rclcpp::Time time_last_active_control_command_received_; - - BridgeState bridge_state_; - ControlMode control_mode_; - int arming_counter_; - double battery_voltage_; - - // Safety flags - bool bridge_armed_; - bool rc_was_disarmed_once_; - - std::atomic_bool destructor_invoked_; - - // Parameters - std::string port_name_; - bool enable_receiving_sbus_messages_; - - double control_command_timeout_; - double rc_timeout_; - - double mass_; - - bool disable_thrust_mapping_; - - double max_roll_rate_; - double max_pitch_rate_; - double max_yaw_rate_; - - double max_roll_angle_; - double max_pitch_angle_; - - double alpha_vbat_filter_; - bool perform_thrust_voltage_compensation_; - int n_lipo_cells_; - - // Constants - static constexpr double kLowLevelFeedbackPublishFrequency_ = 50.0; - - static constexpr int kSmoothingFailRepetitions_ = 5; - - static constexpr double kBatteryLowVoltagePerCell_ = 3.6; - static constexpr double kBatteryCriticalVoltagePerCell_ = 3.4; - static constexpr double kBatteryInvalidVoltagePerCell_ = 3.0; - static constexpr double kBatteryVoltageTimeout_ = 1.0; - - double thrust_vs_rpm_cof_a_; - double thrust_vs_rpm_cof_b_; - double thrust_vs_rpm_cof_c_; - double rpm_vs_throttle_linear_coeff_a_; - double rpm_vs_throttle_linear_coeff_b_; - double rpm_vs_throttle_quadratic_coeff_a_; - double rpm_vs_throttle_quadratic_coeff_b_; - double rpm_vs_throttle_quadratic_coeff_c_; - - bool use_body_rates_; -}; - -} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h deleted file mode 100644 index b6d411f3..00000000 --- a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h +++ /dev/null @@ -1,61 +0,0 @@ -#pragma once - -#include -#include - -namespace sbus_bridge { - -enum class ControlMode { NONE, ATTITUDE, BODY_RATES }; - -enum class ArmState { DISARMED, ARMED }; - -#pragma pack(push) -#pragma pack(1) -struct SBusMsg { - // Constants - static constexpr int kNChannels = 16; - static constexpr uint16_t kMinCmd = 174; // corresponds to 1000 on FC - static constexpr uint16_t kMeanCmd = 992; // corresponds to 1500 on FC - static constexpr uint16_t kMaxCmd = 1811; // corresponds to 2000 on FC - - rclcpp::Time timestamp; - - // Normal 11 bit channels - uint16_t channels[kNChannels]; - - // Digital channels (ch17 and ch18) - bool digital_channel_1; - bool digital_channel_2; - - // Flags - bool frame_lost; - bool failsafe; - - SBusMsg(); -// SBusMsg(const sbus_bridge::SbusRosMessage& sbus_ros_msg); - virtual ~SBusMsg(); - -// sbus_bridge::SbusRosMessage toRosMessage() const; - - void limitAllChannelsFeasible(); - void limitSbusChannelFeasible(const int channel_idx); - - // Setting sbus command helpers - void setThrottleCommand(const uint16_t throttle_cmd); - void setRollCommand(const uint16_t roll_cmd); - void setPitchCommand(const uint16_t pitch_cmd); - void setYawCommand(const uint16_t yaw_cmd); - void setControlMode(const ControlMode& control_mode); - void setControlModeAttitude(); - void setControlModeBodyRates(); - void setArmState(const ArmState& arm_state); - void setArmStateArmed(); - void setArmStateDisarmed(); - - // Sbus message check helpers - bool isArmed() const; - bool isKillSwitch() const; - ControlMode getControlMode() const; -}; -#pragma pack(pop) -} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h deleted file mode 100644 index 25994207..00000000 --- a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h +++ /dev/null @@ -1,52 +0,0 @@ -#pragma once - -#include -#include - -#include "kr_sbus_interface/sbus_msg.h" - -namespace sbus_bridge { - -class SBusSerialPort { - public: - SBusSerialPort(); - SBusSerialPort(const std::string& port, - const bool start_receiver_thread, - const rclcpp::Clock::SharedPtr& clock = nullptr); - virtual ~SBusSerialPort(); - - protected: - bool setUpSBusSerialPort(const std::string& port, - const bool start_receiver_thread, - const rclcpp::Clock::SharedPtr& clock = nullptr); - - bool connectSerialPort(const std::string& port); - void disconnectSerialPort(); - - bool startReceiverThread(); - bool stopReceiverThread(); - - void transmitSerialSBusMessage(const SBusMsg& sbus_msg) const; - virtual void handleReceivedSbusMessage( - const sbus_bridge::SBusMsg& received_sbus_msg) = 0; - - private: - static constexpr int kSbusFrameLength_ = 25; - static constexpr uint8_t kSbusHeaderByte_ = 0x0F; - static constexpr uint8_t kSbusFooterByte_ = 0x00; - static constexpr int kPollTimeoutMilliSeconds_ = 500; - - bool configureSerialPortForSBus() const; - void serialPortReceiveThread(); - sbus_bridge::SBusMsg parseSbusMessage( - uint8_t sbus_msg_bytes[kSbusFrameLength_]) const; - - std::thread receiver_thread_; - std::atomic_bool receiver_thread_should_exit_; - - int serial_port_fd_; - rclcpp::Logger logger_ = rclcpp::get_logger("sbus_serial_port"); - rclcpp::Clock::SharedPtr clock_; -}; - -} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/thrust_mapping.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/thrust_mapping.h deleted file mode 100644 index 9faed5a9..00000000 --- a/interfaces/kr_sbus_interface/include/kr_sbus_interface/thrust_mapping.h +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include - -namespace thrust_mapping { - -class CollectiveThrustMapping { - public: - CollectiveThrustMapping(); - CollectiveThrustMapping(const double thrust_map_a, const double thrust_map_b, - const double thrust_map_c, - const bool perform_thrust_voltage_compensation, - const double thrust_ratio_voltage_map_a, - const double thrust_ratio_voltage_map_b, - const int n_lipo_cells); - - virtual ~CollectiveThrustMapping(); - - uint16_t inverseThrustMapping(const double thrust, - const double battery_voltage) const; - - bool loadParameters(); - - private: - double thrust_map_a_; - double thrust_map_b_; - double thrust_map_c_; - - bool perform_thrust_voltage_compensation_; - double thrust_ratio_voltage_map_a_; - double thrust_ratio_voltage_map_b_; - int n_lipo_cells_; - - // Constants - static constexpr double kMinBatteryCompensationVoltagePerCell_ = 3.5; - static constexpr double kMaxBatteryCompensationVoltagePerCell_ = 4.2; -}; - -} // namespace thrust_mapping diff --git a/interfaces/kr_sbus_interface/launch/control.launch.py b/interfaces/kr_sbus_interface/launch/control.launch.py deleted file mode 100644 index 577e7a83..00000000 --- a/interfaces/kr_sbus_interface/launch/control.launch.py +++ /dev/null @@ -1,213 +0,0 @@ -import os -import yaml -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction, GroupAction, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node, ComposableNodeContainer, PushRosNamespace, LoadComposableNodes -from launch_ros.descriptions import ComposableNode -from launch.conditions import LaunchConfigurationEquals, IfCondition -from launch.substitutions import LaunchConfiguration, PythonExpression, Command, TextSubstitution -from launch_ros.substitutions import FindPackageShare - -def generate_launch_description(): - - # KR interface paths & configurations - config_file = os.path.join( - get_package_share_directory('kr_sbus_interface'), - 'config', - 'neurofly.yaml' - ) - - trackers_manager_config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml' - so3_config_file = os.path.join( - get_package_share_directory('kr_sbus_interface'), - 'config', - 'gains.yaml' - ) - - # URDF/xacro file to be loaded by the Robot State Publisher node - default_xacro_path = os.path.join( - get_package_share_directory('zed_wrapper'), - 'urdf', - 'zed_descr.urdf.xacro' - ) - - # Define launch arguments - vicon_args = [ - # Adding mocap vicon specific parameters - DeclareLaunchArgument('mocap', default_value='false'), - DeclareLaunchArgument('mocap_server', default_value='mocap.perch'), - DeclareLaunchArgument('mocap_frame_rate', default_value='100'), - DeclareLaunchArgument('mocap_max_accel', default_value='10.0'), - ] - - # KR interface arguments - kr_args = [ - DeclareLaunchArgument('robot', default_value='neurofly1'), - DeclareLaunchArgument('odom', default_value='odom'), - DeclareLaunchArgument('so3_cmd', default_value='so3_cmd'), - DeclareLaunchArgument('mass', default_value='.680'), - ] - - # ZED camera arguments - zed_args = [ - DeclareLaunchArgument('zed_enable', default_value='true'), - DeclareLaunchArgument('camera_name', default_value='zed'), - DeclareLaunchArgument('camera_model', default_value='zedm'), - DeclareLaunchArgument('publish_urdf', default_value='true'), - DeclareLaunchArgument('publish_tf', default_value='true'), - DeclareLaunchArgument('publish_map_tf', default_value='true'), - DeclareLaunchArgument('publish_imu_tf', default_value='true'), - DeclareLaunchArgument('xacro_path', default_value=TextSubstitution(text=default_xacro_path)), - DeclareLaunchArgument('custom_baseline', default_value='0.0'), - DeclareLaunchArgument('enable_gnss', default_value='false'), - DeclareLaunchArgument('publish_svo_clock', default_value='false'), - ] - - # Initialize launch description with all arguments - ld = LaunchDescription(vicon_args + kr_args + zed_args) - - # Create a main component container for all composable nodes - main_container = ComposableNodeContainer( - name="control_container", - namespace="", # Empty namespace for container, individual nodes will have their own namespaces - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=[ - ComposableNode( - package="kr_sbus_interface", - plugin="SO3CmdToSBUS", - name="so3cmd_to_sbus", - namespace=LaunchConfiguration('robot'), - parameters=[config_file], - remappings=[ - ('so3_cmd', LaunchConfiguration('so3_cmd')), - ('odom', LaunchConfiguration('odom')), - ('imu', 'zed/zed_node/imu/data'), - ] - ), - ComposableNode( - package="kr_trackers_manager", - plugin="TrackersManager", - name="trackers_manager", - namespace=LaunchConfiguration('robot'), - parameters=[trackers_manager_config_file], - ), - ComposableNode( - package="kr_trackers_manager", - plugin="TrackersManagerLifecycleManager", - name="lifecycle_manager", - namespace=LaunchConfiguration('robot'), - parameters=[ - {'node_name': "trackers_manager"} - ] - ), - ComposableNode( - package="kr_mav_controllers", - plugin="SO3ControlComponent", - name="so3_controller", - namespace=LaunchConfiguration('robot'), - parameters=[so3_config_file], - ), - ComposableNode( - condition=IfCondition(LaunchConfiguration('zed_enable')), - package="zed_components", - plugin="stereolabs::ZedCamera", - name="zed_node", - namespace="zed", - parameters=[ - [FindPackageShare('zed_wrapper').find('zed_wrapper'), '/config/', LaunchConfiguration('camera_model'), '.yaml'], - [FindPackageShare('zed_wrapper').find('zed_wrapper'), '/config/common_stereo.yaml'], - # Finally apply launch-specific overrides - { - 'general.camera_name': LaunchConfiguration('camera_name'), - 'general.camera_model': LaunchConfiguration('camera_model'), - 'pos_tracking.publish_tf': LaunchConfiguration('publish_tf'), - 'pos_tracking.publish_map_tf': LaunchConfiguration('publish_map_tf'), - 'sensors.publish_imu_tf': LaunchConfiguration('publish_imu_tf', default='true'), - } - ], - extra_arguments=[{'use_intra_process_comms': True}] - ), - ComposableNode( - package="quadrotor_ukf_ros2", - plugin="QuadrotorUKFNode", - name="quadrotor_ukf_ros2", - namespace=LaunchConfiguration('robot'), - parameters=[{ - 'odom': LaunchConfiguration('odom'), - 'imu_frame_id': 'zed_imu_link', - 'imu_rotated_frame_id': 'zed_camera_link', - 'base_link': 'zed_camera_link' - }], - remappings=[ - ('odom', '/zed/zed_node/odom'), - ('imu', '/zed/zed_node/imu/data'), - ], - ), - ], - output='screen', - ) - - ld.add_action(main_container) - - # robot state publisher to publish URDF and static transforms for zed - ld.add_action( - Node( - condition=IfCondition(LaunchConfiguration('zed_enable')), - package='robot_state_publisher', - executable='robot_state_publisher', - name=[LaunchConfiguration('camera_name'), '_state_publisher'], - namespace=LaunchConfiguration('robot'), - output='screen', - parameters=[{ - 'use_sim_time': LaunchConfiguration('publish_svo_clock'), - 'robot_description': Command([ - 'xacro', ' ', LaunchConfiguration('xacro_path'), - ' camera_name:=', LaunchConfiguration('camera_name'), - ' camera_model:=', LaunchConfiguration('camera_model'), - ' custom_baseline:=', LaunchConfiguration('custom_baseline') - ]) - }] - ) - ) - - ld.add_action( - Node( - condition=IfCondition(LaunchConfiguration('mocap')), - package='mocap_vicon', - executable='mocap_vicon_node', - name='vicon', - output='screen', - parameters=[ - {'server_address': LaunchConfiguration('mocap_server')}, - {'frame_rate': LaunchConfiguration('mocap_frame_rate')}, - {'max_accel': LaunchConfiguration('mocap_max_accel')}, - {'publish_tf': False}, - {'publish_pts': False}, - {'fixed_frame_id': 'mocap'}, - # Set to [''] to take in ALL models from Vicon - {'model_list': ['neurofly1']}, - ], - remappings=[ - # Uncomment and modify the remapping if needed - ('/neurofly1/odom', '/odom'), - ] - ) - ) - - ld.add_action( - Node( - package="kr_mav_manager", - executable="mav_services", - namespace=LaunchConfiguration('robot'), - name="mav_services", - output='screen', - parameters = [ - {'mass': LaunchConfiguration('mass')}, - ], - ) - ) - - return ld diff --git a/interfaces/kr_sbus_interface/launch/sbus_interface.launch.py b/interfaces/kr_sbus_interface/launch/sbus_interface.launch.py deleted file mode 100644 index 2d068ced..00000000 --- a/interfaces/kr_sbus_interface/launch/sbus_interface.launch.py +++ /dev/null @@ -1,57 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - -def generate_launch_description(): - # Declare launch arguments - robot_ns = LaunchConfiguration('robot', default='/') - odom_topic = LaunchConfiguration('odom', default='odom') - so3_cmd_topic = LaunchConfiguration('so3_cmd', default='so3_cmd') - - # Path to the configuration file - config_file = os.path.join( - get_package_share_directory('kr_sbus_interface'), - 'config', - 'neurofly.yaml' - ) - - # Component container to load your SO3CmdToSbus component - sbus_container = ComposableNodeContainer( - name='so3cmd_to_sbus_container', - namespace=robot_ns, - package='rclcpp_components', - executable='component_container_mt', # multi-threaded - composable_node_descriptions=[ - ComposableNode( - package='kr_sbus_interface', - plugin='SO3CmdToSBUS', - name='so3cmd_to_sbus', - remappings=[ - ('odom', odom_topic), - ('so3_cmd', so3_cmd_topic), - ], - parameters=[config_file], - ), - ], - output='screen', - ) - robot_arg = DeclareLaunchArgument( - 'robot', default_value='' - ) - odom_arg = DeclareLaunchArgument( - 'odom', default_value='odom' - ) - so3_cmd_arg = DeclareLaunchArgument( - 'so3_cmd', default_value='so3_cmd' - ) - return LaunchDescription([ - robot_arg, - odom_arg, - so3_cmd_arg, - sbus_container, - ]) diff --git a/interfaces/kr_sbus_interface/nodelet_plugin.xml b/interfaces/kr_sbus_interface/nodelet_plugin.xml deleted file mode 100644 index 4c71b948..00000000 --- a/interfaces/kr_sbus_interface/nodelet_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - This take a so3_command and sends it to the robot using the SBUS Bridge - - - diff --git a/interfaces/kr_sbus_interface/package.xml b/interfaces/kr_sbus_interface/package.xml deleted file mode 100644 index acc1e2a1..00000000 --- a/interfaces/kr_sbus_interface/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - kr_sbus_interface - 0.0.0 - The kr_sbus_interface package - - Dexter Ong - - BSD - - ament_cmake - - rclcpp - nav_msgs - sensor_msgs - kr_mav_msgs - tf2 - roscpp - rclcpp_components - - ament_cmake - - diff --git a/interfaces/kr_sbus_interface/scripts/mavlink_telemetry.py b/interfaces/kr_sbus_interface/scripts/mavlink_telemetry.py deleted file mode 100644 index 6c70a881..00000000 --- a/interfaces/kr_sbus_interface/scripts/mavlink_telemetry.py +++ /dev/null @@ -1,35 +0,0 @@ -import rospy -from pymavlink import mavutil -from sensor_msgs.msg import BatteryState - -def mavlink_connect(port='/dev/ttyUSB0', baudrate=115200): - """Connect to a MAVLink source""" - return mavutil.mavlink_connection(port, baud=baudrate) - -def battery_status_publisher(): - rospy.init_node('battery_status_publisher', anonymous=True) - battery_pub = rospy.Publisher('battery_status', BatteryState, queue_size=10) - rate = rospy.Rate(100) - - # Connect to MAVLink (adjust port and baudrate as necessary) - mav = mavlink_connect(port='/dev/ttyUSB0', baudrate=115200) - - while not rospy.is_shutdown(): - message = mav.recv_match(blocking=True) - if message: - if message.get_type() == "SYS_STATUS": - battery_msg = BatteryState() - battery_msg.voltage = message.voltage_battery / 1000.0 / 4.0 - battery_msg.current = message.current_battery / 100.0 - battery_msg.percentage = message.battery_remaining - - battery_msg.header.stamp = rospy.Time.now() - battery_pub.publish(battery_msg) - - rate.sleep() - -if __name__ == '__main__': - try: - battery_status_publisher() - except rospy.ROSInterruptException: - pass diff --git a/interfaces/kr_sbus_interface/src/sbus_bridge.cpp b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp deleted file mode 100644 index 55230ffd..00000000 --- a/interfaces/kr_sbus_interface/src/sbus_bridge.cpp +++ /dev/null @@ -1,663 +0,0 @@ -#include - -#include - -#include - -#include - -namespace sbus_bridge { - - SBusBridge::SBusBridge(const rclcpp::Node::SharedPtr& node) - : node_(node), - logger_(node->get_logger()), - stop_watchdog_thread_(false), - time_last_rc_msg_received_(), - time_last_sbus_msg_sent_(node_->now()), - time_last_battery_voltage_received_(node_->now()), - time_last_active_control_command_received_(), - bridge_state_(BridgeState::KILL), - control_mode_(ControlMode::NONE), - arming_counter_(0), - battery_voltage_(0.0), - bridge_armed_(false), - rc_was_disarmed_once_(false), - destructor_invoked_(false) { - - if (!loadParameters()) { - RCLCPP_ERROR(logger_, "Could not load parameters."); - rclcpp::shutdown(); - return; - } - -// sbus_cmd_pub_ = node_->create_publisher("sbus_cmd", 1); - - // Start serial port with receiver thread if receiving sbus messages is - // enabled - if (!setUpSBusSerialPort(port_name_, enable_receiving_sbus_messages_, node->get_clock())) { - rclcpp::shutdown(); - return; - } - - // Start watchdog thread - try { - watchdog_thread_ = std::thread(&SBusBridge::watchdogThread, this); - } catch (...) { - RCLCPP_ERROR( - logger_, - "Could not successfully start watchdog thread. Exiting SBusBridge."); - rclcpp::shutdown(); - return; - } - - RCLCPP_INFO(logger_, "SBusBridge initialized"); -} - -SBusBridge::~SBusBridge() { - destructor_invoked_ = true; - - // Stop SBus receiver thread - if (enable_receiving_sbus_messages_) { - stopReceiverThread(); - } - - // Stop watchdog thread - stop_watchdog_thread_ = true; - // Wait for watchdog thread to finish - watchdog_thread_.join(); - - // Now only one thread (the ROS thread) is remaining - - setBridgeState(BridgeState::OFF); - - // Send disarming SBus message for safety - // We repeat it to prevent any possible smoothing of commands on the flight - // controller to interfere with this - SBusMsg shut_down_message; - shut_down_message.setArmStateDisarmed(); - rclcpp::Rate loop_rate(110.0); - for (int i = 0; i < kSmoothingFailRepetitions_; i++) { - transmitSerialSBusMessage(shut_down_message); - loop_rate.sleep(); - } - - - // Close serial port - disconnectSerialPort(); -} - -void SBusBridge::watchdogThread() { - rclcpp::Rate watchdog_rate(110.0); - while (rclcpp::ok() && !stop_watchdog_thread_) { - watchdog_rate.sleep(); - - std::lock_guard main_lock(main_mutex_); - - const rclcpp::Time time_now = node_->now(); - - if (bridge_state_ == BridgeState::RC_FLIGHT && - (time_now - time_last_rc_msg_received_).seconds() > rc_timeout_) { - // If the last received RC command was armed but was received longer than - // rc_timeout ago we switch the bridge state to AUTONOMOUS_FLIGHT. - // In case there are no valid control commands the bridge state is set to - // OFF in the next check below - RCLCPP_WARN( - logger_, - "Remote control was active but no message from it was received " - "within timeout (%f s).", - rc_timeout_); - setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); - } - - // if (bridge_state_ == BridgeState::ARMING || - // bridge_state_ == BridgeState::AUTONOMOUS_FLIGHT) { - // if ((time_now - time_last_active_control_command_received_).seconds() > - // control_command_timeout_) { - // // When switching the bridge state to off, our watchdog ensures that a - // // disarming off message is repeated. - // RCLCPP_INFO(node_->get_logger(), "No active control command received, setting bridge state to OFF"); - // setBridgeState(BridgeState::OFF); - // // Note: Control could theoretically still be taken over by RC but if - // // this happened in flight it might require super human reaction since - // // in this case the quad can not be armed with non zero throttle by - // // the remote. - // } - // } - - if (bridge_state_ == BridgeState::KILL) { - // Send off message that disarms the vehicle - // We repeat it to prevent any weird behavior that occurs if the flight - // controller is not receiving commands for a while - SBusMsg kill_msg; - kill_msg.setArmStateDisarmed(); - sendSBusMessageToSerialPort(kill_msg); - // disarm bridge if needed - if (bridge_armed_) { - disarmBridge(); - } - } - - // Check battery voltage timeout - std::lock_guard battery_lock(battery_voltage_mutex_); - if ((time_now - time_last_battery_voltage_received_).seconds() > - kBatteryVoltageTimeout_) { - battery_voltage_ = 0.0; - if (perform_thrust_voltage_compensation_) { - RCLCPP_WARN_THROTTLE( - logger_, *node_->get_clock(), 1000, - "Can not perform battery voltage compensation because there " - "is no recent battery voltage measurement"); - } - } - - // Mutexes are unlocked because they go out of scope here - } -} - -void SBusBridge::handleReceivedSbusMessage(const SBusMsg &received_sbus_msg) -{ - { - std::lock_guard main_lock(main_mutex_); - - time_last_rc_msg_received_ = node_->now(); - - if (received_sbus_msg.isKillSwitch()) - { - if (bridge_state_ != BridgeState::KILL) - { - setBridgeState(BridgeState::KILL); - RCLCPP_INFO(logger_, "Kill switch ON"); - } - } - else - { - if (bridge_state_ == BridgeState::KILL) - { - setBridgeState(BridgeState::OFF); - RCLCPP_WARN(logger_, "Kill switch OFF"); - } - } - - if (bridge_state_ == BridgeState::KILL) - { - // send disarm to FC to kill motors - // sendSBusMessageToSerialPort(received_sbus_msg); - - return; - } - - if (received_sbus_msg.isArmed()) - { - if (!rc_was_disarmed_once_) { - // This flag prevents that the vehicle can be armed if the RC is armed - // on startup of the bridge - RCLCPP_WARN_THROTTLE( - logger_, *node_->get_clock(), 1.0, - "RC needs to be disarmed once before it can take over control"); - return; - } - - // Immediately go into RC_FLIGHT state since RC always has priority - if (bridge_state_ != BridgeState::RC_FLIGHT) { - setBridgeState(BridgeState::RC_FLIGHT); - RCLCPP_INFO(logger_, "Control authority taken over by remote control."); - } - sendSBusMessageToSerialPort(received_sbus_msg); - control_mode_ = received_sbus_msg.getControlMode(); - } - else if (bridge_state_ == BridgeState::RC_FLIGHT) - { - // If the bridge was in state RC_FLIGHT and the RC is disarmed we set the - // state to AUTONOMOUS_FLIGHT - // In case there are valid control commands, the bridge will stay in - // AUTONOMOUS_FLIGHT, otherwise the watchdog will set the state to OFF - RCLCPP_INFO(logger_, "Control authority returned by remote control."); - if (bridge_armed_) - { - RCLCPP_INFO(logger_, "Bridge armed, setting bridge state to AUTONOMOUS_FLIGHT"); - setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); - } - else - { - // When switching the bridge state to off, our watchdog ensures that a - // disarming off message is sent - RCLCPP_INFO(logger_, "Bridge not armed, setting bridge state to OFF"); - setBridgeState(BridgeState::OFF); - } - } - else if (!rc_was_disarmed_once_) - { - RCLCPP_INFO( - logger_, - "Received disarmed RC message but RC was not disarmed once, ignoring it"); - rc_was_disarmed_once_ = true; - } - else if (bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) { - // not killed, not armed, not autonomous flight - sendSBusMessageToSerialPort(received_sbus_msg); - } - - // Main mutex is unlocked here because it goes out of scope - } - -// received_sbus_msg_pub_.publish(received_sbus_msg.toRosMessage()); -} - -void SBusBridge::controlCommandCallback( - const kr_mav_msgs::msg::SO3Command::ConstPtr &msg, - const Eigen::Quaterniond &odom_q) -{ - std::lock_guard main_lock(main_mutex_); - - if (destructor_invoked_) - { - // This ensures that if the destructor was invoked we do not try to write - // to the serial port anymore because of receiving a control command - return; - } - - // may need more logic @TODO - time_last_active_control_command_received_ = node_->now(); - - if (!bridge_armed_ || bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) - { - // If bridge is not armed we do not allow control commands to be sent - // RC has priority over control commands for autonomous flying - if (!bridge_armed_ && msg->aux.enable_motors && - bridge_state_ != BridgeState::RC_FLIGHT) - { - RCLCPP_WARN( - logger_, - "Received active control command but sbus bridge is not armed. " - "Please arm the bridge before sending control commands."); - } - return; - } - - SBusMsg sbus_msg_to_send; - { - std::lock_guard battery_lock(battery_voltage_mutex_); - // Set commands - sbus_msg_to_send = generateSBusMessageFromSO3Command(msg, odom_q); - // Battery voltage mutex is unlocked because it goes out of scope here - } - - // Send disarm if necessary - if (!msg->aux.enable_motors) - { - if (bridge_state_ == BridgeState::ARMING || - bridge_state_ == BridgeState::AUTONOMOUS_FLIGHT) - { - RCLCPP_INFO(logger_, "Control command received, setting bridge state to OFF"); - setBridgeState(BridgeState::OFF); - } - // Make sure vehicle is disarmed to immediately switch it off - sbus_msg_to_send.setArmStateDisarmed(); - } - - // Limit channels - sbus_msg_to_send.limitAllChannelsFeasible(); - - // Immediately send SBus message - sendSBusMessageToSerialPort(sbus_msg_to_send); - - // Main mutex is unlocked because it goes out of scope here -} - -void SBusBridge::sendSBusMessageToSerialPort(const SBusMsg& sbus_msg) -{ - // // print bridge state - // switch (bridge_state_) { - // case BridgeState::OFF: - // ROS_INFO("Bridge state: OFF"); - // break; - // case BridgeState::ARMING: - // ROS_INFO("Bridge state: ARMING"); - // break; - // case BridgeState::AUTONOMOUS_FLIGHT: - // ROS_INFO("Bridge state: AUTONOMOUS_FLIGHT"); - // break; - // case BridgeState::RC_FLIGHT: - // ROS_INFO("Bridge state: RC_FLIGHT"); - // break; - // default: - // ROS_INFO("Bridge state: UNKNOWN"); - // } - SBusMsg sbus_message_to_send = sbus_msg; - - switch (bridge_state_) { - case BridgeState::OFF: - // Disarm vehicle - sbus_message_to_send.setArmStateDisarmed(); - // ROS_INFO("Bridge state: OFF"); - break; - - case BridgeState::ARMING: - // Since there might be some RC commands smoothing and checks on multiple - // messages before arming, we repeat the messages with minimum throttle - // and arming command multiple times. 5 times seems to work robustly. - if (arming_counter_ >= kSmoothingFailRepetitions_) { - setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); - } else { - // Set thrust to minimum command to make sure FC arms - sbus_message_to_send.setThrottleCommand(SBusMsg::kMinCmd); - sbus_message_to_send.setArmStateArmed(); - arming_counter_++; - } - break; - - case BridgeState::AUTONOMOUS_FLIGHT: - sbus_message_to_send.setArmStateArmed(); - if (use_body_rates_) - { - sbus_message_to_send.setControlModeBodyRates(); - } else - { - sbus_message_to_send.setControlModeAttitude(); - } - break; - - case BridgeState::RC_FLIGHT: - // Passing RC command straight through - // ROS_INFO("RC MODE: throttle_cmd %d", sbus_message_to_send.channels[sbus_bridge::channel_mapping::kThrottle]); - sbus_message_to_send.setControlModeAttitude(); - break; - - case BridgeState::KILL: - // Disarm vehicle - sbus_message_to_send.setArmStateDisarmed(); - // ROS_INFO("Bridge state: OFF"); - break; - - default: - // Disarm the vehicle because this code must be terribly wrong - sbus_message_to_send.setArmStateDisarmed(); - RCLCPP_WARN( - logger_, "Bridge is in unknown state, vehicle will be disarmed"); - break; - } - - if ((node_->now() - time_last_sbus_msg_sent_).seconds() <= 0.006) { - // An SBUS message takes 3ms to be transmitted by the serial port so let's - // not stress it too much. This should only happen in case of switching - // between control commands and rc commands - if (bridge_state_ == BridgeState::ARMING) { - // In case of arming we want to send kSmoothingFailRepetitions_ messages - // with minimum throttle to the flight controller. Since this check - // prevents the message from being sent out we reduce the counter that - // was incremented above assuming the message would actually be sent. - arming_counter_--; - } - return; - } - - sbus_message_to_send.timestamp = node_->now(); - transmitSerialSBusMessage(sbus_message_to_send); - time_last_sbus_msg_sent_ = node_->now(); -} - -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)); -} - -double SBusBridge::thrust_model_kartik(double thrust) const -{ - int num_props = 4; - double avg_thrust = std::max(0.0, thrust) / num_props; - - // Scale thrust to individual rotor velocities (RPM) - 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 betaflight - double throttle = - (omega_avg - rpm_vs_throttle_linear_coeff_b_) / rpm_vs_throttle_linear_coeff_a_; - return throttle; -} - -SBusMsg SBusBridge::generateSBusMessageFromSO3Command(const kr_mav_msgs::msg::SO3Command::ConstPtr& msg, const Eigen::Quaterniond& odom_q) const -{ - SBusMsg sbus_msg; - - // set sbus_msg to not killed - sbus_msg.channels[sbus_bridge::channel_mapping::kKillSwitch] = 1792; - - sbus_msg.setArmStateArmed(); - - // 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); - // const Eigen::Vector3d ang_vel(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); - - // convert to tf::Quaternion - // tf::Quaternion imu_tf = tf::Quaternion(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w()); - // tf::Quaternion odom_tf = tf::Quaternion(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w()); - - const Eigen::Matrix3d R_cur(odom_q); - - double thrust = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); - - double throttle = 0.0; - if (thrust > 1e-5) - { - throttle = thrust_model_kartik(thrust); - } - - // remap throttle (1000 to 2000) to sbus (kMinCmd to kMaxCmd) - uint16_t throttle_cmd = round(((throttle - 1000) / 1000) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); - RCLCPP_INFO_THROTTLE( - logger_, *node_->get_clock(), 1.0, - "AUTONOMOUS MODE: thrust: %f throttle: %f throttle_cmd: %d", - thrust, throttle, throttle_cmd); - sbus_msg.setThrottleCommand(throttle_cmd); - - // convert quaternion to euler - Eigen::Vector3d desired_euler_angles = q_des.toRotationMatrix().eulerAngles(0, 1, 2); - - // sanity check if larger than 3 rad, set to 0 - for (int i = 0; i < 3; i++) { - if (desired_euler_angles(i) > 3.0 || desired_euler_angles(i) < -3.0) - { - desired_euler_angles(i) = 0; - } - } - - // parse commands - uint16_t roll_cmd, pitch_cmd, yaw_cmd; - - if (use_body_rates_) - { - // get body rates - double roll_rate = msg->angular_velocity.x; - double pitch_rate = msg->angular_velocity.y; - - roll_rate = std::max(-max_roll_rate_, std::min(max_roll_rate_, roll_rate)); - pitch_rate = std::max(-max_pitch_rate_, std::min(max_pitch_rate_, pitch_rate)); - - // additional safety check - roll_rate = std::max(-400.0, std::min(400.0, roll_rate)); - pitch_rate = std::max(-400.0, std::min(400.0, pitch_rate)); - - roll_cmd = round((roll_rate + max_roll_rate_) / (2 * max_roll_rate_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); - pitch_cmd = round((pitch_rate + max_pitch_rate_) / (2 * max_pitch_rate_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); - - RCLCPP_INFO_THROTTLE( - logger_, *node_->get_clock(), 1000, - "AUTONOMOUS MODE: roll_rate: %f pitch_rate: %f roll_cmd: %d pitch_cmd: %d", - roll_rate, pitch_rate, roll_cmd, pitch_cmd); - } - else - { - // get body angles - double yaw, roll, pitch; - // use tf2 to convert quaternion to euler - tf2::Matrix3x3(tf2::Quaternion(q_des.x(), q_des.y(), q_des.z(), q_des.w())).getRPY(roll, pitch, yaw); - // convert to degrees - double roll_deg = roll * 180.0 / M_PI; - double pitch_deg = pitch * 180.0 / M_PI; - - // clip max roll - roll_deg = std::max(-max_roll_angle_, std::min(max_roll_angle_, roll_deg)); - // clip max pitch - pitch_deg = std::max(-max_pitch_angle_, std::min(max_pitch_angle_, pitch_deg)); - - roll_cmd = round((roll_deg + max_roll_angle_) / (2 * max_roll_angle_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); - pitch_cmd = round((pitch_deg + max_pitch_angle_) / (2 * max_pitch_angle_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); - - RCLCPP_INFO_THROTTLE( - logger_, *node_->get_clock(), 1000, - "AUTONOMOUS MODE: roll_deg: %f pitch_deg: %f roll_cmd: %d pitch_cmd: %d", - roll_deg, pitch_deg, roll_cmd, pitch_cmd); - } - - // always use yaw rate - double yaw_rate = -msg->angular_velocity.z; // flip sign since FC yaw is inverted (z-down) - yaw_rate = yaw_rate * 180.0 / M_PI; // convert to degrees - yaw_rate = std::max(-400.0, std::min(400.0, yaw_rate)); // clip max yaw rate - yaw_cmd = round((yaw_rate + max_yaw_rate_) / (2 * max_yaw_rate_) * (SBusMsg::kMaxCmd - SBusMsg::kMinCmd) + SBusMsg::kMinCmd); - -// RCLCPP_INFO_THROTTLE( -// logger_, *node_->get_clock(), 1.0, -// "AUTONOMOUS MODE: yaw_rate: %f yaw_cmd: %d", -// yaw_rate, yaw_cmd); - - sbus_msg.setRollCommand(roll_cmd); - sbus_msg.setPitchCommand(pitch_cmd); - sbus_msg.setYawCommand(yaw_cmd); - -// // publish values as odom message -// nav_msgs::Odometry sbus_cmd; -// sbus_cmd.header.stamp = ros::Time::now(); -// // convert to float -// sbus_cmd.pose.pose.position.x = static_cast(roll_cmd); -// sbus_cmd.pose.pose.position.y = static_cast(pitch_cmd); -// sbus_cmd.pose.pose.position.z = static_cast(yaw_cmd); - -// // publish -// sbus_cmd_pub_.publish(sbus_cmd); - -// ROS_INFO("Thrust: %f", thrust_mapping_.inverseThrustMapping( -// thrust * mass_, battery_voltage_)); - // ROS_INFO("Roll: %f", round((desired_euler_angles(0) / max_roll_angle_) * - // (SBusMsg::kMaxCmd - SBusMsg::kMeanCmd) + - // SBusMsg::kMeanCmd)); - // ROS_INFO("Pitch: %f", round((desired_euler_angles(1) / max_pitch_angle_) * - // (SBusMsg::kMaxCmd - SBusMsg::kMeanCmd) + - // SBusMsg::kMeanCmd)); - // ROS_INFO("Yaw: %f", round((msg->angular_velocity.z / max_yaw_rate_) * - // (SBusMsg::kMaxCmd - SBusMsg::kMeanCmd) + - // SBusMsg::kMeanCmd)); - // ROS_INFO("Desired Euler Angles: %f, %f, %f", desired_euler_angles(0), desired_euler_angles(1), desired_euler_angles(2)); - // ROS_INFO("Current Euler Angles: %f, %f, %f", odom_q.toRotationMatrix().eulerAngles(0, 1, 2)(0), odom_q.toRotationMatrix().eulerAngles(0, 1, 2)(1), odom_q.toRotationMatrix().eulerAngles(0, 1, 2)(2)); - return sbus_msg; -} - -void SBusBridge::setBridgeState(const BridgeState& desired_bridge_state) { - switch (desired_bridge_state) { - case BridgeState::OFF: - bridge_state_ = desired_bridge_state; - break; - - case BridgeState::ARMING: - bridge_state_ = desired_bridge_state; - arming_counter_ = 0; - break; - - case BridgeState::AUTONOMOUS_FLIGHT: - bridge_state_ = desired_bridge_state; - break; - - case BridgeState::RC_FLIGHT: - bridge_state_ = desired_bridge_state; - break; - - case BridgeState::KILL: - bridge_state_ = desired_bridge_state; - break; - - default: - RCLCPP_WARN( - logger_, "Wanted to switch to unknown bridge state, setting to OFF"); - bridge_state_ = BridgeState::OFF; - } -} - -void SBusBridge::armBridge() -{ - std::lock_guard main_lock(arm_mutex_); - bridge_armed_ = true; -} - -void SBusBridge::disarmBridge() -{ - std::lock_guard main_lock(arm_mutex_); - bridge_armed_ = false; -} - -bool SBusBridge::isBridgeArmed() const -{ - return bridge_armed_; -} - -bool SBusBridge::loadParameters() -{ - node_->declare_parameter("port_name", std::string("/dev/ttyUSB0")); - node_->declare_parameter("enable_receiving_sbus_messages", true); - node_->declare_parameter("control_command_timeout", 0.5); - node_->declare_parameter("rc_timeout", 0.5); - node_->declare_parameter("mass", 1.0); - node_->declare_parameter("disable_thrust_mapping", false); - node_->declare_parameter("max_roll_rate", 400.0); - node_->declare_parameter("max_pitch_rate", 400.0); - node_->declare_parameter("max_yaw_rate", 400.0); - node_->declare_parameter("max_roll_angle", 0.5); - node_->declare_parameter("max_pitch_angle", 0.5); - node_->declare_parameter("alpha_vbat_filter", 0.1); - node_->declare_parameter("perform_thrust_voltage_compensation", true); - node_->declare_parameter("n_lipo_cells", 3); - node_->declare_parameter("thrust_vs_rpm_cof_a_", 0.0001); - node_->declare_parameter("thrust_vs_rpm_cof_b_", 0.0); - node_->declare_parameter("thrust_vs_rpm_cof_c_", 0.0); - node_->declare_parameter("rpm_vs_throttle_linear_coeff_a_", 0.0001); - node_->declare_parameter("rpm_vs_throttle_linear_coeff_b_", 0.0); - node_->declare_parameter("rpm_vs_throttle_quadratic_coeff_a_", 0.0001); - node_->declare_parameter("rpm_vs_throttle_quadratic_coeff_b_", 0.0); - node_->declare_parameter("rpm_vs_throttle_quadratic_coeff_c_", 0.0); - node_->declare_parameter("use_body_rates", false); - - // Get parameters - if (!node_->get_parameter("port_name", port_name_) || - !node_->get_parameter("enable_receiving_sbus_messages", enable_receiving_sbus_messages_) || - !node_->get_parameter("control_command_timeout", control_command_timeout_) || - !node_->get_parameter("rc_timeout", rc_timeout_) || - !node_->get_parameter("mass", mass_) || - !node_->get_parameter("disable_thrust_mapping", disable_thrust_mapping_) || - !node_->get_parameter("max_roll_rate", max_roll_rate_) || - !node_->get_parameter("max_pitch_rate", max_pitch_rate_) || - !node_->get_parameter("max_yaw_rate", max_yaw_rate_) || - !node_->get_parameter("max_roll_angle", max_roll_angle_) || - !node_->get_parameter("max_pitch_angle", max_pitch_angle_) || - !node_->get_parameter("alpha_vbat_filter", alpha_vbat_filter_) || - !node_->get_parameter("perform_thrust_voltage_compensation", perform_thrust_voltage_compensation_) || - !node_->get_parameter("n_lipo_cells", n_lipo_cells_) || - !node_->get_parameter("thrust_vs_rpm_cof_a_", thrust_vs_rpm_cof_a_) || - !node_->get_parameter("thrust_vs_rpm_cof_b_", thrust_vs_rpm_cof_b_) || - !node_->get_parameter("thrust_vs_rpm_cof_c_", thrust_vs_rpm_cof_c_) || - !node_->get_parameter("rpm_vs_throttle_linear_coeff_a_", rpm_vs_throttle_linear_coeff_a_) || - !node_->get_parameter("rpm_vs_throttle_linear_coeff_b_", rpm_vs_throttle_linear_coeff_b_) || - !node_->get_parameter("rpm_vs_throttle_quadratic_coeff_a_", rpm_vs_throttle_quadratic_coeff_a_) || - !node_->get_parameter("rpm_vs_throttle_quadratic_coeff_b_", rpm_vs_throttle_quadratic_coeff_b_) || - !node_->get_parameter("rpm_vs_throttle_quadratic_coeff_c_", rpm_vs_throttle_quadratic_coeff_c_) || - !node_->get_parameter("use_body_rates", use_body_rates_)) { - RCLCPP_ERROR(logger_, "Failed to load one or more parameters."); - return false; - } - return true; -} - -} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/src/sbus_msg.cpp b/interfaces/kr_sbus_interface/src/sbus_msg.cpp deleted file mode 100644 index fea65423..00000000 --- a/interfaces/kr_sbus_interface/src/sbus_msg.cpp +++ /dev/null @@ -1,112 +0,0 @@ -#include "kr_sbus_interface/sbus_msg.h" - -#include "kr_sbus_interface/channel_mapping.h" - -namespace sbus_bridge { - -SBusMsg::SBusMsg() - : timestamp(rclcpp::Clock().now()), - digital_channel_1(false), - digital_channel_2(false), - frame_lost(false), - failsafe(false) { - for (int i = 0; i < kNChannels; i++) { - channels[i] = kMeanCmd; - } -} - -SBusMsg::~SBusMsg() {} - -void SBusMsg::limitAllChannelsFeasible() { - for (uint8_t i = 0; i < kNChannels; i++) { - limitSbusChannelFeasible(i); - } -} - -void SBusMsg::limitSbusChannelFeasible(const int channel_idx) { - if (channel_idx < 0 || channel_idx >= kNChannels) { - return; - } - - if (channels[channel_idx] > kMaxCmd) { - channels[channel_idx] = kMaxCmd; - } - if (channels[channel_idx] < kMinCmd) { - channels[channel_idx] = kMinCmd; - } -} - -void SBusMsg::setThrottleCommand(const uint16_t throttle_cmd) { - channels[channel_mapping::kThrottle] = throttle_cmd; -} - -void SBusMsg::setRollCommand(const uint16_t roll_cmd) { - channels[channel_mapping::kRoll] = roll_cmd; -} - -void SBusMsg::setPitchCommand(const uint16_t pitch_cmd) { - channels[channel_mapping::kPitch] = pitch_cmd; -} - -void SBusMsg::setYawCommand(const uint16_t yaw_cmd) { - channels[channel_mapping::kYaw] = yaw_cmd; -} - -void SBusMsg::setControlMode(const ControlMode& control_mode) { - if (control_mode == ControlMode::ATTITUDE) { - channels[channel_mapping::kControlMode] = kMinCmd; - } else if (control_mode == ControlMode::BODY_RATES) { - channels[channel_mapping::kControlMode] = kMaxCmd; - } -} - -void SBusMsg::setControlModeAttitude() { - setControlMode(ControlMode::ATTITUDE); -} - -void SBusMsg::setControlModeBodyRates() { - setControlMode(ControlMode::BODY_RATES); -} - -void SBusMsg::setArmState(const ArmState& arm_state) { - if (arm_state == ArmState::ARMED) { - channels[channel_mapping::kArming] = kMaxCmd; - } else { - channels[channel_mapping::kArming] = kMinCmd; - } -} - -void SBusMsg::setArmStateArmed() { setArmState(ArmState::ARMED); } - -void SBusMsg::setArmStateDisarmed() { - setArmState(ArmState::DISARMED); - // Should not be necessary but for safety we also set the throttle command - // to the minimum - setThrottleCommand(kMinCmd); -} - -bool SBusMsg::isArmed() const { - if (channels[channel_mapping::kArming] <= kMeanCmd) { - return false; - } - - return true; -} - -bool SBusMsg::isKillSwitch() const { - if (channels[channel_mapping::kKillSwitch] > kMeanCmd) { - return false; - } - - return true; -} - -ControlMode SBusMsg::getControlMode() const { - if (channels[channel_mapping::kControlMode] > kMeanCmd) { - return ControlMode::BODY_RATES; - } - - return ControlMode::ATTITUDE; -} - -} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp b/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp deleted file mode 100644 index 20cc7ca3..00000000 --- a/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp +++ /dev/null @@ -1,409 +0,0 @@ -#include "kr_sbus_interface/sbus_serial_port.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace sbus_bridge { - -SBusSerialPort::SBusSerialPort() - : receiver_thread_(), - receiver_thread_should_exit_(false), - serial_port_fd_(-1), - clock_(nullptr) { - logger_ = rclcpp::get_logger("sbus_serial_port"); -} - -SBusSerialPort::SBusSerialPort(const std::string& port, - const bool start_receiver_thread, - const rclcpp::Clock::SharedPtr& clock) - : receiver_thread_(), - receiver_thread_should_exit_(false), - serial_port_fd_(-1), - clock_(clock) { - logger_ = rclcpp::get_logger("sbus_serial_port"); -} - -SBusSerialPort::~SBusSerialPort() { disconnectSerialPort(); } - -bool SBusSerialPort::setUpSBusSerialPort(const std::string& port, - const bool start_receiver_thread, - const rclcpp::Clock::SharedPtr& clock) { - clock_ = clock; - if (!connectSerialPort(port)) { - return false; - } - - if (start_receiver_thread) { - if (!startReceiverThread()) { - return false; - } - } - - return true; -} - -bool SBusSerialPort::connectSerialPort(const std::string& port) { - // Open serial port - // O_RDWR - Read and write - // O_NOCTTY - Ignore special chars like CTRL-C - serial_port_fd_ = open(port.c_str(), O_RDWR | O_NOCTTY); - - if (serial_port_fd_ == -1) { - RCLCPP_ERROR(logger_, "Could not open serial port %s", port.c_str()); - return false; - } - - if (!configureSerialPortForSBus()) { - close(serial_port_fd_); - RCLCPP_ERROR(logger_, "Could not set necessary configuration of serial port"); - return false; - } - - RCLCPP_INFO(logger_, "Connected to serial port %s", port.c_str()); - return true; -} - -void SBusSerialPort::disconnectSerialPort() { - stopReceiverThread(); - if (serial_port_fd_ != -1) { - close(serial_port_fd_); - serial_port_fd_ = -1; - } -} - -bool SBusSerialPort::startReceiverThread() { - // Start watchdog thread - try { - receiver_thread_ = - std::thread(&SBusSerialPort::serialPortReceiveThread, this); - } catch (...) { - RCLCPP_ERROR(logger_, "Could not successfully start SBUS receiver thread."); - return false; - } - - return true; -} - -bool SBusSerialPort::stopReceiverThread() { - if (!receiver_thread_.joinable()) { - return true; - } - - receiver_thread_should_exit_ = true; - - // Wait for receiver thread to finish - receiver_thread_.join(); - - return true; -} - -bool SBusSerialPort::configureSerialPortForSBus() const { - // clear config - fcntl(serial_port_fd_, F_SETFL, 0); - // read non blocking - fcntl(serial_port_fd_, F_SETFL, FNDELAY); - - struct termios2 uart_config; - /* Fill the struct for the new configuration */ - ioctl(serial_port_fd_, TCGETS2, &uart_config); - - // Output flags - Turn off output processing - // no CR to NL translation, no NL to CR-NL translation, - // no NL to CR translation, no column 0 CR suppression, - // no Ctrl-D suppression, no fill characters, no case mapping, - // no local output processing - // - uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); - - // Input flags - Turn off input processing - // convert break to null byte, no CR to NL translation, - // no NL to CR translation, don't mark parity errors or breaks - // no input parity check, don't strip high bit off, - // no XON/XOFF software flow control - // - uart_config.c_iflag &= - ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); - - // - // No line processing: - // echo off - // echo newline off - // canonical mode off, - // extended input processing off - // signal chars off - // - uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - - // Turn off character processing - // Turn off odd parity - uart_config.c_cflag &= ~(CSIZE | PARODD | CBAUD); - - // Enable parity generation on output and parity checking for input. - uart_config.c_cflag |= PARENB; - // Set two stop bits, rather than one. - uart_config.c_cflag |= CSTOPB; - // No output processing, force 8 bit input - uart_config.c_cflag |= CS8; - // Enable a non standard baud rate - uart_config.c_cflag |= BOTHER; - - // Set custom baud rate of 100'000 bits/s necessary for sbus - const speed_t spd = 100000; - uart_config.c_ispeed = spd; - uart_config.c_ospeed = spd; - - if (ioctl(serial_port_fd_, TCSETS2, &uart_config) < 0) { - RCLCPP_ERROR(logger_, "Could not set configuration of serial port"); - return false; - } - - return true; -} - -void SBusSerialPort::transmitSerialSBusMessage(const SBusMsg& sbus_msg) const { - static uint8_t buffer[kSbusFrameLength_]; - - // SBUS header - buffer[0] = kSbusHeaderByte_; - - // 16 channels of 11 bit data - buffer[1] = (uint8_t)((sbus_msg.channels[0] & 0x07FF)); - buffer[2] = (uint8_t)((sbus_msg.channels[0] & 0x07FF) >> 8 | - (sbus_msg.channels[1] & 0x07FF) << 3); - buffer[3] = (uint8_t)((sbus_msg.channels[1] & 0x07FF) >> 5 | - (sbus_msg.channels[2] & 0x07FF) << 6); - buffer[4] = (uint8_t)((sbus_msg.channels[2] & 0x07FF) >> 2); - buffer[5] = (uint8_t)((sbus_msg.channels[2] & 0x07FF) >> 10 | - (sbus_msg.channels[3] & 0x07FF) << 1); - buffer[6] = (uint8_t)((sbus_msg.channels[3] & 0x07FF) >> 7 | - (sbus_msg.channels[4] & 0x07FF) << 4); - buffer[7] = (uint8_t)((sbus_msg.channels[4] & 0x07FF) >> 4 | - (sbus_msg.channels[5] & 0x07FF) << 7); - buffer[8] = (uint8_t)((sbus_msg.channels[5] & 0x07FF) >> 1); - buffer[9] = (uint8_t)((sbus_msg.channels[5] & 0x07FF) >> 9 | - (sbus_msg.channels[6] & 0x07FF) << 2); - buffer[10] = (uint8_t)((sbus_msg.channels[6] & 0x07FF) >> 6 | - (sbus_msg.channels[7] & 0x07FF) << 5); - buffer[11] = (uint8_t)((sbus_msg.channels[7] & 0x07FF) >> 3); - buffer[12] = (uint8_t)((sbus_msg.channels[8] & 0x07FF)); - buffer[13] = (uint8_t)((sbus_msg.channels[8] & 0x07FF) >> 8 | - (sbus_msg.channels[9] & 0x07FF) << 3); - buffer[14] = (uint8_t)((sbus_msg.channels[9] & 0x07FF) >> 5 | - (sbus_msg.channels[10] & 0x07FF) << 6); - buffer[15] = (uint8_t)((sbus_msg.channels[10] & 0x07FF) >> 2); - buffer[16] = (uint8_t)((sbus_msg.channels[10] & 0x07FF) >> 10 | - (sbus_msg.channels[11] & 0x07FF) << 1); - buffer[17] = (uint8_t)((sbus_msg.channels[11] & 0x07FF) >> 7 | - (sbus_msg.channels[12] & 0x07FF) << 4); - buffer[18] = (uint8_t)((sbus_msg.channels[12] & 0x07FF) >> 4 | - (sbus_msg.channels[13] & 0x07FF) << 7); - buffer[19] = (uint8_t)((sbus_msg.channels[13] & 0x07FF) >> 1); - buffer[20] = (uint8_t)((sbus_msg.channels[13] & 0x07FF) >> 9 | - (sbus_msg.channels[14] & 0x07FF) << 2); - buffer[21] = (uint8_t)((sbus_msg.channels[14] & 0x07FF) >> 6 | - (sbus_msg.channels[15] & 0x07FF) << 5); - buffer[22] = (uint8_t)((sbus_msg.channels[15] & 0x07FF) >> 3); - - // SBUS flags - // (bit0 = least significant bit) - // bit0 = ch17 = digital channel (0x01) - // bit1 = ch18 = digital channel (0x02) - // bit2 = Frame lost, equivalent red LED on receiver (0x04) - // bit3 = Failsafe activated (0x08) - // bit4 = n/a - // bit5 = n/a - // bit6 = n/a - // bit7 = n/a - buffer[23] = 0x00; - if (sbus_msg.digital_channel_1) { - buffer[23] |= 0x01; - } - if (sbus_msg.digital_channel_2) { - buffer[23] |= 0x02; - } - if (sbus_msg.frame_lost) { - buffer[23] |= 0x04; - } - if (sbus_msg.failsafe) { - buffer[23] |= 0x08; - } - - // SBUS footer - buffer[24] = kSbusFooterByte_; - - const int written = write(serial_port_fd_, (char*)buffer, kSbusFrameLength_); - // tcflush(serial_port_fd_, TCOFLUSH); // There were rumors that this might - // not work on Odroids... - if (written != kSbusFrameLength_) { - RCLCPP_ERROR(logger_, "Wrote %d bytes but should have written %d", written, kSbusFrameLength_); - } -} - -void SBusSerialPort::serialPortReceiveThread() { - struct pollfd fds[1]; - fds[0].fd = serial_port_fd_; - fds[0].events = POLLIN; - - uint8_t init_buf[10]; - while (read(serial_port_fd_, init_buf, sizeof(init_buf)) > 0) { - // On startup, as long as we receive something, we keep reading to ensure - // that the first byte of the first poll is the start of an SBUS message - // and not some arbitrary byte. - // This should help to get the framing in sync in the beginning. - usleep(100); - } - - std::deque bytes_buf; - - while (!receiver_thread_should_exit_) { - // Buffer to read bytes from serial port. We make it large enough to - // potentially contain 4 sbus messages but its actual size probably does - // not matter too much - uint8_t read_buf[4 * kSbusFrameLength_]; - - if (poll(fds, 1, kPollTimeoutMilliSeconds_) > 0) { - if (fds[0].revents & POLLIN) { - const ssize_t nread = read(serial_port_fd_, read_buf, sizeof(read_buf)); - - for (ssize_t i = 0; i < nread; i++) { - bytes_buf.push_back(read_buf[i]); - } - - bool valid_sbus_message_received = false; - uint8_t sbus_msg_bytes[kSbusFrameLength_]; - while (bytes_buf.size() >= kSbusFrameLength_) { - // Check if we have a potentially valid SBUS message - // A valid SBUS message must have to correct header and footer byte - // as well as zeros in the four most significant bytes of the flag - // byte (byte 23) - if (bytes_buf.front() == kSbusHeaderByte_ && - !(bytes_buf[kSbusFrameLength_ - 2] & 0xF0) && - bytes_buf[kSbusFrameLength_ - 1] == kSbusFooterByte_) { - for (uint8_t i = 0; i < kSbusFrameLength_; i++) { - sbus_msg_bytes[i] = bytes_buf.front(); - bytes_buf.pop_front(); - } - - valid_sbus_message_received = true; - } else { - // If it is not a valid SBUS message but has a correct header byte - // we need to pop it to prevent staying in this loop forever - bytes_buf.pop_front(); - RCLCPP_WARN(logger_, "SBUS message framing not in sync"); - } - - // If not, pop front elements until we have a valid header byte - while (!bytes_buf.empty() && bytes_buf.front() != kSbusHeaderByte_) { - bytes_buf.pop_front(); - } - } - - if (valid_sbus_message_received) { - // Sometimes we read more than one sbus message at the same time - // By running the loop above for as long as possible before handling - // the received sbus message we achieve to only process the latest one - const SBusMsg received_sbus_msg = parseSbusMessage(sbus_msg_bytes); - handleReceivedSbusMessage(received_sbus_msg); - } - } - } - } - - return; -} - -SBusMsg SBusSerialPort::parseSbusMessage( - uint8_t sbus_msg_bytes[kSbusFrameLength_]) const { - SBusMsg sbus_msg; - - if (clock_) { - sbus_msg.timestamp = clock_->now(); - } else { - sbus_msg.timestamp = rclcpp::Clock().now(); - } - - // Decode the 16 regular channels - sbus_msg.channels[0] = - (((uint16_t)sbus_msg_bytes[1]) | ((uint16_t)sbus_msg_bytes[2] << 8)) & - 0x07FF; - sbus_msg.channels[1] = (((uint16_t)sbus_msg_bytes[2] >> 3) | - ((uint16_t)sbus_msg_bytes[3] << 5)) & - 0x07FF; - sbus_msg.channels[2] = - (((uint16_t)sbus_msg_bytes[3] >> 6) | ((uint16_t)sbus_msg_bytes[4] << 2) | - ((uint16_t)sbus_msg_bytes[5] << 10)) & - 0x07FF; - sbus_msg.channels[3] = (((uint16_t)sbus_msg_bytes[5] >> 1) | - ((uint16_t)sbus_msg_bytes[6] << 7)) & - 0x07FF; - sbus_msg.channels[4] = (((uint16_t)sbus_msg_bytes[6] >> 4) | - ((uint16_t)sbus_msg_bytes[7] << 4)) & - 0x07FF; - sbus_msg.channels[5] = - (((uint16_t)sbus_msg_bytes[7] >> 7) | ((uint16_t)sbus_msg_bytes[8] << 1) | - ((uint16_t)sbus_msg_bytes[9] << 9)) & - 0x07FF; - sbus_msg.channels[6] = (((uint16_t)sbus_msg_bytes[9] >> 2) | - ((uint16_t)sbus_msg_bytes[10] << 6)) & - 0x07FF; - sbus_msg.channels[7] = (((uint16_t)sbus_msg_bytes[10] >> 5) | - ((uint16_t)sbus_msg_bytes[11] << 3)) & - 0x07FF; - sbus_msg.channels[8] = - (((uint16_t)sbus_msg_bytes[12]) | ((uint16_t)sbus_msg_bytes[13] << 8)) & - 0x07FF; - sbus_msg.channels[9] = (((uint16_t)sbus_msg_bytes[13] >> 3) | - ((uint16_t)sbus_msg_bytes[14] << 5)) & - 0x07FF; - sbus_msg.channels[10] = (((uint16_t)sbus_msg_bytes[14] >> 6) | - ((uint16_t)sbus_msg_bytes[15] << 2) | - ((uint16_t)sbus_msg_bytes[16] << 10)) & - 0x07FF; - sbus_msg.channels[11] = (((uint16_t)sbus_msg_bytes[16] >> 1) | - ((uint16_t)sbus_msg_bytes[17] << 7)) & - 0x07FF; - sbus_msg.channels[12] = (((uint16_t)sbus_msg_bytes[17] >> 4) | - ((uint16_t)sbus_msg_bytes[18] << 4)) & - 0x07FF; - sbus_msg.channels[13] = (((uint16_t)sbus_msg_bytes[18] >> 7) | - ((uint16_t)sbus_msg_bytes[19] << 1) | - ((uint16_t)sbus_msg_bytes[20] << 9)) & - 0x07FF; - sbus_msg.channels[14] = (((uint16_t)sbus_msg_bytes[20] >> 2) | - ((uint16_t)sbus_msg_bytes[21] << 6)) & - 0x07FF; - sbus_msg.channels[15] = (((uint16_t)sbus_msg_bytes[21] >> 5) | - ((uint16_t)sbus_msg_bytes[22] << 3)) & - 0x07FF; - - // Decode flags from byte 23 (see comments in sendSBusMessage function) - // By default, flags are set to false in SBusMsg constructor, so we only need - // to set them true if necessary - if (sbus_msg_bytes[23] & (1 << 0)) { - sbus_msg.digital_channel_1 = true; - } - - if (sbus_msg_bytes[23] & (1 << 1)) { - sbus_msg.digital_channel_2 = true; - } - - if (sbus_msg_bytes[23] & (1 << 2)) { - sbus_msg.frame_lost = true; - } - - if (sbus_msg_bytes[23] & (1 << 3)) { - sbus_msg.failsafe = true; - } - - return sbus_msg; -} - -} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_component.cpp b/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_component.cpp deleted file mode 100644 index 62c12cd4..00000000 --- a/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_component.cpp +++ /dev/null @@ -1,156 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -using namespace std::chrono_literals; - -class SO3CmdToSBUS : public rclcpp::Node -{ -public: - explicit SO3CmdToSBUS(const rclcpp::NodeOptions &options); - -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 check_so3_cmd_timeout(); - void motors_on(); - void motors_off(); - - // Controller state - bool odom_set_, imu_set_, so3_cmd_set_; - Eigen::Quaterniond odom_q_, imu_q_; - - // Subscribers - rclcpp::Subscription::SharedPtr so3_cmd_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; -// rclcpp::Subscription::SharedPtr imu_sub_; - - // Motor status - int motor_status_; - - // Timeout handling[18][21] - double so3_cmd_timeout_; - rclcpp::Time last_so3_cmd_time_; - kr_mav_msgs::msg::SO3Command last_so3_cmd_; - - // SBUS bridge - std::shared_ptr sbus_bridge_; - rclcpp::TimerBase::SharedPtr init_timer_; -}; - -SO3CmdToSBUS::SO3CmdToSBUS(const rclcpp::NodeOptions &options) - : Node("so3cmd_to_sbus", rclcpp::NodeOptions(options).use_intra_process_comms(true)), - odom_set_(false), - imu_set_(false), - so3_cmd_set_(false), - motor_status_(0) -{ - this->declare_parameter("so3_cmd_timeout", 0.25); - so3_cmd_timeout_ = this->get_parameter("so3_cmd_timeout").as_double(); - - // Create subscribers - so3_cmd_sub_ = this->create_subscription( - "so3_cmd", 10, - std::bind(&SO3CmdToSBUS::so3_cmd_callback, this, std::placeholders::_1)); - - odom_sub_ = this->create_subscription( - "odom", 10, - std::bind(&SO3CmdToSBUS::odom_callback, this, std::placeholders::_1)); - -// imu_sub_ = this->create_subscription( -// "imu", 10, -// std::bind(&SO3CmdToSBUS::imu_callback, this, std::placeholders::_1)); - - init_timer_ = this->create_wall_timer( - std::chrono::milliseconds(0), - [this]() { - this->init_timer_->cancel(); - sbus_bridge_ = std::make_shared(shared_from_this()); - } - ); - - RCLCPP_INFO(this->get_logger(), "SO3CmdToSbus component initialized"); -} - -void SO3CmdToSBUS::so3_cmd_callback(const kr_mav_msgs::msg::SO3Command::SharedPtr msg) -{ - if (!so3_cmd_set_) - so3_cmd_set_ = true; - - // Switch on motors - if (msg->aux.enable_motors && !sbus_bridge_->isBridgeArmed() && !motor_status_) - { - motors_on(); - } - else if (!msg->aux.enable_motors) - { - motors_off(); - } - - sbus_bridge_->controlCommandCallback(msg, odom_q_); - - // Save last so3_cmd - last_so3_cmd_ = *msg; - last_so3_cmd_time_ = this->now(); -} - -void SO3CmdToSBUS::odom_callback(const nav_msgs::msg::Odometry::SharedPtr odom) -{ - if (!odom_set_) - odom_set_ = true; - - odom_q_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, - odom->pose.pose.orientation.x, - odom->pose.pose.orientation.y, - odom->pose.pose.orientation.z); - - check_so3_cmd_timeout(); -} - -void SO3CmdToSBUS::imu_callback(const sensor_msgs::msg::Imu::SharedPtr pose) -{ - if (!imu_set_) - imu_set_ = true; - - imu_q_ = Eigen::Quaterniond(pose->orientation.w, pose->orientation.x, - pose->orientation.y, pose->orientation.z); - - check_so3_cmd_timeout(); -} - -void SO3CmdToSBUS::check_so3_cmd_timeout() -{ - if (so3_cmd_set_ && ((this->now() - last_so3_cmd_time_).seconds() >= so3_cmd_timeout_)) - { - RCLCPP_DEBUG(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 SO3CmdToSBUS::motors_on() -{ - RCLCPP_INFO(this->get_logger(), "Motors on"); - sbus_bridge_->armBridge(); - motor_status_ = 1; -} - -void SO3CmdToSBUS::motors_off() -{ - sbus_bridge_->disarmBridge(); - motor_status_ = 0; -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(SO3CmdToSBUS) diff --git a/interfaces/kr_sbus_interface/src/thrust_mapping.cpp b/interfaces/kr_sbus_interface/src/thrust_mapping.cpp deleted file mode 100644 index e0b8a2a8..00000000 --- a/interfaces/kr_sbus_interface/src/thrust_mapping.cpp +++ /dev/null @@ -1,85 +0,0 @@ -#include "kr_sbus_interface/thrust_mapping.h" - -#include - -namespace thrust_mapping -{ - -CollectiveThrustMapping::CollectiveThrustMapping() - : thrust_map_a_(0.0), - thrust_map_b_(0.0), - thrust_map_c_(0.0), - perform_thrust_voltage_compensation_(false), - thrust_ratio_voltage_map_a_(0.0), - thrust_ratio_voltage_map_b_(0.0), - n_lipo_cells_(0) -{ -} - -CollectiveThrustMapping::CollectiveThrustMapping(const double thrust_map_a, const double thrust_map_b, - const double thrust_map_c, - const bool perform_thrust_voltage_compensation, - const double thrust_ratio_voltage_map_a, - const double thrust_ratio_voltage_map_b, const int n_lipo_cells) - : thrust_map_a_(thrust_map_a), - thrust_map_b_(thrust_map_b), - thrust_map_c_(thrust_map_c), - perform_thrust_voltage_compensation_(perform_thrust_voltage_compensation), - thrust_ratio_voltage_map_a_(thrust_ratio_voltage_map_a), - thrust_ratio_voltage_map_b_(thrust_ratio_voltage_map_b), - n_lipo_cells_(n_lipo_cells) -{ -} - -CollectiveThrustMapping::~CollectiveThrustMapping() {} - -uint16_t CollectiveThrustMapping::inverseThrustMapping(const double thrust, const double battery_voltage) const -{ - double thrust_applied = thrust; - if(perform_thrust_voltage_compensation_) - { - if(battery_voltage >= n_lipo_cells_ * kMinBatteryCompensationVoltagePerCell_ && - battery_voltage <= n_lipo_cells_ * kMaxBatteryCompensationVoltagePerCell_) - { - const double thrust_cmd_voltage_ratio = - thrust_ratio_voltage_map_a_ * battery_voltage + thrust_ratio_voltage_map_b_; - thrust_applied *= thrust_cmd_voltage_ratio; - } - else - { - ROS_WARN_THROTTLE(1.0, "[%s] Battery voltage out of range for compensation", ros::this_node::getName().c_str()); - } - } - - // Citardauq Formula: Gives a numerically stable solution of the quadratic equation for thrust_map_a ~ 0, which is not - // the case for the standard formula. - const uint16_t cmd = - 2.0 * (thrust_map_c_ - thrust_applied) / - (-thrust_map_b_ - sqrt(thrust_map_b_ * thrust_map_b_ - 4.0 * thrust_map_a_ * (thrust_map_c_ - thrust_applied))); - - return cmd; -} - -bool CollectiveThrustMapping::loadParameters() -{ - ros::NodeHandle pnh("~"); - - if(!pnh.getParam("thrust_map_a", thrust_map_a_)) - return false; - if(!pnh.getParam("thrust_map_b", thrust_map_b_)) - return false; - if(!pnh.getParam("thrust_map_c", thrust_map_c_)) - return false; - if(!pnh.getParam("perform_thrust_voltage_compensation", perform_thrust_voltage_compensation_)) - return false; - if(!pnh.getParam("thrust_ratio_voltage_map_a", thrust_ratio_voltage_map_a_)) - return false; - if(!pnh.getParam("thrust_ratio_voltage_map_b", thrust_ratio_voltage_map_b_)) - return false; - if(!pnh.getParam("n_lipo_cells", n_lipo_cells_)) - return false; - - return true; -} - -} // namespace thrust_mapping From cb1e813e8929ea19f70562bc64db14ec4ba097a2 Mon Sep 17 00:00:00 2001 From: Dexter Date: Fri, 10 Oct 2025 13:56:41 -0400 Subject: [PATCH 45/64] Fixed namespaced relative topics --- .../launch/control.launch.py | 16 ++++++++-------- kr_mav_controllers/src/so3_control_component.cpp | 8 ++++---- kr_mav_manager/src/manager.cpp | 6 +++--- .../config/trackers_manager.yaml | 2 +- .../kr_trackers_manager/launch/example.launch.py | 9 +++++++-- .../kr_trackers_manager/src/trackers_manager.cpp | 2 +- 6 files changed, 24 insertions(+), 19 deletions(-) diff --git a/interfaces/kr_betaflight_interface/launch/control.launch.py b/interfaces/kr_betaflight_interface/launch/control.launch.py index 7acf1813..f5467fa0 100644 --- a/interfaces/kr_betaflight_interface/launch/control.launch.py +++ b/interfaces/kr_betaflight_interface/launch/control.launch.py @@ -14,14 +14,14 @@ def generate_launch_description(): # KR interface paths & configurations config_file = os.path.join( - get_package_share_directory('kr_crsf_interface'), + get_package_share_directory('kr_betaflight_interface'), 'config', 'neurofly.yaml' ) trackers_manager_config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml' so3_config_file = os.path.join( - get_package_share_directory('kr_crsf_interface'), + get_package_share_directory('kr_betaflight_interface'), 'config', 'gains.yaml' ) @@ -76,9 +76,9 @@ def generate_launch_description(): executable="component_container_mt", composable_node_descriptions=[ ComposableNode( - package="kr_crsf_interface", - plugin="SO3CmdToCRSF", - name="so3cmd_to_crsf", + package="kr_betaflight_interface", + plugin="SO3CmdToBetaflight", + name="so3cmd_to_betaflight", namespace=LaunchConfiguration('robot'), parameters=[config_file], remappings=[ @@ -115,7 +115,7 @@ def generate_launch_description(): package="zed_components", plugin="stereolabs::ZedCamera", name="zed_node", - namespace="zed", + namespace=LaunchConfiguration('robot'), parameters=[ [FindPackageShare('zed_wrapper').find('zed_wrapper'), '/config/', LaunchConfiguration('camera_model'), '.yaml'], [FindPackageShare('zed_wrapper').find('zed_wrapper'), '/config/common_stereo.yaml'], @@ -142,8 +142,8 @@ def generate_launch_description(): 'base_link': 'zed_camera_link' }], remappings=[ - ('odom', '/zed/zed_node/odom'), - ('imu', '/zed/zed_node/imu/data'), + ('odom', 'zed_node/odom'), + ('imu', 'zed_node/imu/data'), ], ), ], diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index 5105ca7c..c1b9b375 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -644,13 +644,13 @@ SO3ControlComponent::SO3ControlComponent(const rclcpp::NodeOptions &options) auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 2), qos_profile); odom_sub_ = this->create_subscription( - "/cf3/odom", qos1, std::bind(&SO3ControlComponent::odom_callback, this, std::placeholders::_1)); + "control_odom", qos1, std::bind(&SO3ControlComponent::odom_callback, this, std::placeholders::_1)); position_cmd_sub_ = this->create_subscription( - "/cf3/trackers_manager/cmd", qos1, std::bind(&SO3ControlComponent::position_cmd_callback, this, std::placeholders::_1)); + "trackers_manager/cmd", qos1, std::bind(&SO3ControlComponent::position_cmd_callback, this, std::placeholders::_1)); enable_motors_sub_ = this->create_subscription( - "/cf3/motors", qos2, std::bind(&SO3ControlComponent::enable_motors_callback, this, std::placeholders::_1)); + "motors", qos2, std::bind(&SO3ControlComponent::enable_motors_callback, this, std::placeholders::_1)); corrections_sub_ = this->create_subscription( - "/cf3/corrections", qos1, std::bind(&SO3ControlComponent::corrections_callback, this, std::placeholders::_1)); + "corrections", qos1, std::bind(&SO3ControlComponent::corrections_callback, this, std::placeholders::_1)); } #include "rclcpp_components/register_node_macro.hpp" diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 99bbc4a4..b48ec08d 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -102,9 +102,9 @@ MAVManager::MAVManager() // Publishers pub_motors_ = this->create_publisher("motors", 10); pub_estop_ = this->create_publisher("estop", 10); - pub_so3_command_ = this->create_publisher("/cf3/so3_controller/so3_cmd", 10); + pub_so3_command_ = this->create_publisher("so3_controller/so3_cmd", 10); pub_trpy_command_ = this->create_publisher("trpy_cmd", 10); - pub_position_command_ = this->create_publisher("/cf3/trackers_manager/cmd", 10); + pub_position_command_ = this->create_publisher("trackers_manager/cmd", 10); pub_status_ = this->create_publisher("~/status", 10); pub_goal_velocity_ = this->create_publisher("trackers_manager/velocity_tracker/goal", 10); @@ -115,7 +115,7 @@ MAVManager::MAVManager() // Subscribers odom_sub_ = - this->create_subscription("odom", qos, std::bind(&MAVManager::odometry_cb, this, _1)); + this->create_subscription("control_odom", qos, std::bind(&MAVManager::odometry_cb, this, _1)); heartbeat_sub_ = this->create_subscription("heartbeat", qos, std::bind(&MAVManager::heartbeat_cb, this, _1)); tracker_status_sub_ = this->create_subscription( diff --git a/trackers/kr_trackers_manager/config/trackers_manager.yaml b/trackers/kr_trackers_manager/config/trackers_manager.yaml index 8856c360..22d73b55 100644 --- a/trackers/kr_trackers_manager/config/trackers_manager.yaml +++ b/trackers/kr_trackers_manager/config/trackers_manager.yaml @@ -1,4 +1,4 @@ -cf3/trackers_manager: +trackers_manager: ros__parameters: trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk"] line_tracker_distance/default_v_des: 0.5 diff --git a/trackers/kr_trackers_manager/launch/example.launch.py b/trackers/kr_trackers_manager/launch/example.launch.py index 99dae6ea..097305ca 100644 --- a/trackers/kr_trackers_manager/launch/example.launch.py +++ b/trackers/kr_trackers_manager/launch/example.launch.py @@ -20,9 +20,12 @@ def generate_launch_description(): robot_arg = DeclareLaunchArgument( 'namespace', default_value='' ) - config_file_arg = DeclareLaunchArgument('config_file') + trackers_manager_config_file_arg = DeclareLaunchArgument('config_file') # Path to the configuration file - config_file = LaunchConfiguration('config_file') + trackers_manager_config_file = LaunchConfiguration('trackers_manager_config_file_arg') + + so3_config_file_arg = DeclareLaunchArgument('so3_config_file') + so3_config_file = LaunchConfiguration('so3_config_file') container = ComposableNodeContainer( name="trackers_manager_container", @@ -49,6 +52,7 @@ def generate_launch_description(): package="kr_mav_controllers", plugin="SO3ControlComponent", name="so3_controller", + parameters=[so3_config_file], ), ], output='screen' @@ -58,6 +62,7 @@ def generate_launch_description(): return LaunchDescription([ robot_arg, config_file_arg, + so3_config_file_arg, OpaqueFunction(function=print_config_file), # Call the function to print config_file container, ]) diff --git a/trackers/kr_trackers_manager/src/trackers_manager.cpp b/trackers/kr_trackers_manager/src/trackers_manager.cpp index 58fce3a7..544c9fb5 100644 --- a/trackers/kr_trackers_manager/src/trackers_manager.cpp +++ b/trackers/kr_trackers_manager/src/trackers_manager.cpp @@ -100,7 +100,7 @@ LifecycleCallbackReturn TrackersManager::on_configure(const rclcpp_lifecycle::St // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - sub_odom_ = this->create_subscription("/cf3/odom", qos, std::bind(&TrackersManager::odom_callback, this, std::placeholders::_1)); + sub_odom_ = this->create_subscription("control_odom", qos, std::bind(&TrackersManager::odom_callback, this, std::placeholders::_1)); srv_tracker_ = this->create_service("~/transition", std::bind(&TrackersManager::transition_callback, this, std::placeholders::_1, std::placeholders::_2)); From 3b26130db075ce53726ec303ad9719a02bea42ea Mon Sep 17 00:00:00 2001 From: Dexter Date: Fri, 10 Oct 2025 16:21:59 -0400 Subject: [PATCH 46/64] Cleaned up some config and namespace issues --- .../kr_betaflight_interface/config/gains.yaml | 41 +++++++++++++------ .../config/trackers.yaml | 21 +++++----- .../launch/control.launch.py | 14 ++++--- .../src/so3cmd_to_betaflight_component.cpp | 35 ++++++++++++---- .../src/so3_control_component.cpp | 4 +- 5 files changed, 75 insertions(+), 40 deletions(-) diff --git a/interfaces/kr_betaflight_interface/config/gains.yaml b/interfaces/kr_betaflight_interface/config/gains.yaml index f8868f19..8903bad7 100644 --- a/interfaces/kr_betaflight_interface/config/gains.yaml +++ b/interfaces/kr_betaflight_interface/config/gains.yaml @@ -1,15 +1,32 @@ -gains: - pos: {x: 3.4, y: 3.4, z: 5.4} - vel: {x: 2.8, y: 2.8, z: 3.0} - ki: {x: 0.00, y: 0.00, z: 0.00, yaw: 0.01} - kib: {x: 0.00, y: 0.00, z: 0.00} - rot: {x: 1.5, y: 1.5, z: 1.0} - ang: {x: 0.13, y: 0.13, z: 0.1} - -corrections: - kf: 0.0e-08 - r: 0.0 - p: 0.0 +# neurofly1 +quadrotor_name: neurofly1 +mass: 0.68 + +kp_x: 3.4 +kp_y: 3.4 +kp_z: 5.4 +kd_x: 2.8 +kd_y: 2.8 +kd_z: 3.0 +ki_x: 0.00 +ki_y: 0.00 +ki_z: 0.00 + +kib_x: 0.00 +kib_y: 0.00 +kib_z: 0.00 + +rot_x: 1.5 +rot_y: 1.5 +rot_z: 1.0 + +ang_x: 0.13 +ang_y: 0.13 +ang_z: 0.1 + +kf_correction: 0.0e-08 +roll_correction: 0.0 +pitch_correction: 0.0 max_pos_int: 0.5 max_pos_int_b: 0.5 \ 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 9fc1551d..8d48c1d7 100644 --- a/interfaces/kr_betaflight_interface/config/trackers.yaml +++ b/interfaces/kr_betaflight_interface/config/trackers.yaml @@ -1,11 +1,10 @@ -trackers: - - kr_trackers/LineTrackerMinJerk - - kr_trackers/LineTrackerDistance -# - kr_trackers/LineTrackerTrapezoid -# - kr_trackers/LineTrackerYaw - - kr_trackers/VelocityTrackerAction - - kr_trackers/NullTracker - - kr_trackers/CircleTracker - - kr_trackers/TrajectoryTracker - - kr_trackers/SmoothVelTracker - - kr_trackers/LissajousTracker +/neurofly1/trackers_manager: + ros__parameters: + trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk"] + line_tracker_distance/default_v_des: 1.0 + line_tracker_distance/default_a_des: 0.5 + line_tracker_distance/epsilon: 0.1 + line_tracker_min_jerk/default_v_des: 1.0 + line_tracker_min_jerk/default_a_des: 0.3 + line_tracker_min_jerk/default_yaw_v_des: 0.8 + line_tracker_min_jerk/default_yaw_a_des: 0.2 diff --git a/interfaces/kr_betaflight_interface/launch/control.launch.py b/interfaces/kr_betaflight_interface/launch/control.launch.py index f5467fa0..e14f2954 100644 --- a/interfaces/kr_betaflight_interface/launch/control.launch.py +++ b/interfaces/kr_betaflight_interface/launch/control.launch.py @@ -19,7 +19,7 @@ def generate_launch_description(): 'neurofly.yaml' ) - trackers_manager_config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml' + trackers_manager_config_file = FindPackageShare('kr_betaflight_interface').find('kr_betaflight_interface') + '/config/trackers.yaml' so3_config_file = os.path.join( get_package_share_directory('kr_betaflight_interface'), 'config', @@ -44,10 +44,10 @@ def generate_launch_description(): # KR interface arguments kr_args = [ - DeclareLaunchArgument('robot', default_value='neurofly1'), - DeclareLaunchArgument('odom', default_value='odom'), + DeclareLaunchArgument('robot', default_value='neurofly1'), # set robot namespace + DeclareLaunchArgument('mass', default_value='.680'), # set mass AUW + DeclareLaunchArgument('odom', default_value='control_odom'), # set odom topic (vio/ukf/vicon) DeclareLaunchArgument('so3_cmd', default_value='so3_cmd'), - DeclareLaunchArgument('mass', default_value='.680'), ] # ZED camera arguments @@ -108,7 +108,9 @@ def generate_launch_description(): plugin="SO3ControlComponent", name="so3_controller", namespace=LaunchConfiguration('robot'), - parameters=[so3_config_file], + parameters=[ + [so3_config_file], + ], ), ComposableNode( condition=IfCondition(LaunchConfiguration('zed_enable')), @@ -188,7 +190,7 @@ def generate_launch_description(): {'publish_pts': False}, {'fixed_frame_id': 'mocap'}, # Set to [''] to take in ALL models from Vicon - {'model_list': ['neurofly1']}, + {'model_list': [LaunchConfiguration('robot')]}, ], remappings=[ # Uncomment and modify the remapping if needed diff --git a/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp b/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp index b4830080..ba1285e9 100644 --- a/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp +++ b/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp @@ -22,13 +22,12 @@ class SO3CmdToBetaflight : public rclcpp::Node 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 check_so3_cmd_timeout(); void motors_on(); void motors_off(); // Controller state - bool odom_set_, imu_set_, so3_cmd_set_; + bool odom_set_, so3_cmd_set_; Eigen::Quaterniond odom_q_; // Subscribers @@ -52,7 +51,6 @@ class SO3CmdToBetaflight : public rclcpp::Node SO3CmdToBetaflight::SO3CmdToBetaflight(const rclcpp::NodeOptions &options) : Node("so3cmd_to_bridge", rclcpp::NodeOptions(options).use_intra_process_comms(true)), odom_set_(false), - imu_set_(false), so3_cmd_set_(false), motor_status_(0) { @@ -91,19 +89,28 @@ void SO3CmdToBetaflight::so3_cmd_callback(const kr_mav_msgs::msg::SO3Command::Sh { if (!so3_cmd_set_) so3_cmd_set_ = true; + + if (!bridge_) + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "Bridge not initialized yet."); + return; + } - // Switch on motors - if (msg->aux.enable_motors && bridge_ && !bridge_->isBridgeArmed() && !motor_status_) + // switch on motors + if (msg->aux.enable_motors && !bridge_->isBridgeArmed() && !motor_status_ && odom_set_) { motors_on(); } - else if (!msg->aux.enable_motors && bridge_) + else if (!msg->aux.enable_motors) { motors_off(); } - if (bridge_) + // only send if valid odom + if (odom_set_) + { bridge_->controlCommandCallback(msg, odom_q_); + } // Save last so3_cmd last_so3_cmd_ = *msg; @@ -117,14 +124,23 @@ void SO3CmdToBetaflight::odom_callback(const nav_msgs::msg::Odometry::SharedPtr odom->pose.pose.orientation.x, odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); - odom_set_ = true; + // check if odom is valid + if (std::isnan(odom_q_.w()) || std::isnan(odom_q_.x()) || std::isnan(odom_q_.y()) || std::isnan(odom_q_.z())) + { + RCLCPP_WARN(this->get_logger(), "Received nan in odom orientation"); + odom_set_ = false; + } + else + { + odom_set_ = true; + } } void SO3CmdToBetaflight::check_so3_cmd_timeout() { if (so3_cmd_set_ && ((this->now() - last_so3_cmd_time_).seconds() >= so3_cmd_timeout_)) { - RCLCPP_DEBUG(this->get_logger(), "so3_cmd timeout. %f seconds since last command", + RCLCPP_WARN(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_); @@ -134,6 +150,7 @@ void SO3CmdToBetaflight::check_so3_cmd_timeout() void SO3CmdToBetaflight::motors_on() { + if (bridge_) bridge_->armBridge(); motor_status_ = 1; } diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index c1b9b375..d4aa7fd3 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -635,8 +635,8 @@ SO3ControlComponent::SO3ControlComponent(const rclcpp::NodeOptions &options) reconfigure_handle_ = this->add_on_set_parameters_callback(std::bind(&SO3ControlComponent::cfg_callback, this, std::placeholders::_1)); - so3_command_pub_ = this->create_publisher("~/so3_cmd", 10); - command_viz_pub_ = this->create_publisher("~/cmd_viz", 10); + so3_command_pub_ = this->create_publisher("so3_cmd", 10); + command_viz_pub_ = this->create_publisher("cmd_viz", 10); // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; From 33f8b4257c6b5ec6f1111fdda550d966cdc84aa7 Mon Sep 17 00:00:00 2001 From: Dexter Date: Fri, 10 Oct 2025 16:57:18 -0400 Subject: [PATCH 47/64] Updated arg --- trackers/kr_trackers_manager/launch/example.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/trackers/kr_trackers_manager/launch/example.launch.py b/trackers/kr_trackers_manager/launch/example.launch.py index 097305ca..fbafd1f4 100644 --- a/trackers/kr_trackers_manager/launch/example.launch.py +++ b/trackers/kr_trackers_manager/launch/example.launch.py @@ -61,7 +61,7 @@ def generate_launch_description(): return LaunchDescription([ robot_arg, - config_file_arg, + trackers_manager_config_file_arg, so3_config_file_arg, OpaqueFunction(function=print_config_file), # Call the function to print config_file container, From fb0f6e7ca9dda9eb69e4f54308231af7a189d860 Mon Sep 17 00:00:00 2001 From: Dexter Date: Fri, 10 Oct 2025 16:57:41 -0400 Subject: [PATCH 48/64] Fixed param arg --- trackers/kr_trackers_manager/launch/example.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/trackers/kr_trackers_manager/launch/example.launch.py b/trackers/kr_trackers_manager/launch/example.launch.py index fbafd1f4..af5f11c9 100644 --- a/trackers/kr_trackers_manager/launch/example.launch.py +++ b/trackers/kr_trackers_manager/launch/example.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): package="kr_trackers_manager", plugin="TrackersManager", name="trackers_manager", - parameters=[config_file], + parameters=[trackers_manager_config_file], ), ComposableNode( package="kr_trackers_manager", From 93a92b124af264e3392e8c2e1f132f0b1607db59 Mon Sep 17 00:00:00 2001 From: Dexter Date: Fri, 10 Oct 2025 17:17:47 -0400 Subject: [PATCH 49/64] Removed build test --- .github/workflows/build_and_test.yml | 42 ---------------------------- 1 file changed, 42 deletions(-) delete mode 100644 .github/workflows/build_and_test.yml diff --git a/.github/workflows/build_and_test.yml b/.github/workflows/build_and_test.yml deleted file mode 100644 index 93802fdb..00000000 --- a/.github/workflows/build_and_test.yml +++ /dev/null @@ -1,42 +0,0 @@ -name: build-and-test - -on: - push: - branches: [master] - pull_request: - branches: [master] - -jobs: - build: - strategy: - matrix: - ros_distro: [melodic, noetic] - - runs-on: ubuntu-latest - container: osrf/ros:${{ matrix.ros_distro }}-desktop - steps: - - uses: actions/checkout@v2 - - - name: Install dependencies - run: | - apt-get update - apt-get install -qy g++ libeigen3-dev python3-catkin-tools - rosdep update - rosdep install --from-paths . --ignore-src -y -r --as-root apt:false - - - name: Setup catkin workspace - run: | - . /opt/ros/${{ matrix.ros_distro }}/setup.sh - mkdir -p ${RUNNER_WORKSPACE}/catkin_ws/src - cd ${RUNNER_WORKSPACE}/catkin_ws/src - ln -s ${GITHUB_WORKSPACE} - catkin_init_workspace . - - - name: Build workspace and run tests - run: | - . /opt/ros/${{ matrix.ros_distro }}/setup.sh - cd ${RUNNER_WORKSPACE}/catkin_ws - export LDFLAGS="-Wl,-O1,--sort-common,--as-needed,--no-undefined,-z,relro,-z,now -pthread" - catkin build -j4 --no-status -DCMAKE_BUILD_TYPE=Release - . ${RUNNER_WORKSPACE}/catkin_ws/devel/setup.sh - catkin test From 709eb10cdec5ccd3521af518c78a9fc5d4414423 Mon Sep 17 00:00:00 2001 From: Dexter Date: Tue, 14 Oct 2025 09:35:43 -0400 Subject: [PATCH 50/64] Added poly tracker --- trackers/kr_tracker_msgs/CMakeLists.txt | 2 + .../kr_tracker_msgs/action/PolyTracker.action | 30 + trackers/kr_tracker_msgs/msg/Polynomial.msg | 5 + trackers/kr_trackers/src/poly_tracker.cpp | 638 ++++++++++++++++++ 4 files changed, 675 insertions(+) create mode 100644 trackers/kr_tracker_msgs/action/PolyTracker.action create mode 100644 trackers/kr_tracker_msgs/msg/Polynomial.msg create mode 100644 trackers/kr_trackers/src/poly_tracker.cpp diff --git a/trackers/kr_tracker_msgs/CMakeLists.txt b/trackers/kr_tracker_msgs/CMakeLists.txt index 3e3f9158..aaf3b854 100644 --- a/trackers/kr_tracker_msgs/CMakeLists.txt +++ b/trackers/kr_tracker_msgs/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/TrackerStatus.msg" "msg/VelocityGoal.msg" +"msg/Polynomial.msg" "srv/Transition.srv" @@ -34,6 +35,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/LissajousTracker.action" "action/TrajectoryTracker.action" "action/VelocityTracker.action" +"action/PolyTracker.action" DEPENDENCIES std_msgs builtin_interfaces geometry_msgs ) diff --git a/trackers/kr_tracker_msgs/action/PolyTracker.action b/trackers/kr_tracker_msgs/action/PolyTracker.action new file mode 100644 index 00000000..18ad081a --- /dev/null +++ b/trackers/kr_tracker_msgs/action/PolyTracker.action @@ -0,0 +1,30 @@ +#goal definition +builtin_interfaces/Time t_start +uint8 order +bool set_yaw +bool separate_yaw +float64 start_yaw +float64 final_yaw +######################## polynomial seting +kr_tracker_msgs/Polynomial[] seg_x +kr_tracker_msgs/Polynomial[] seg_y +kr_tracker_msgs/Polynomial[] seg_z +kr_tracker_msgs/Polynomial[] seg_yaw +######################## bspline / berstein seting +int8 cpts_status # 0 is no valid cpts, 1 is berstein, 2 is bspline +float32[] knots +geometry_msgs/Point[] pos_pts +float64[] yaw_pts +######################## added for discrete flat state +geometry_msgs/Point[] vel_pts ### must set to enable +geometry_msgs/Point[] acc_pts +float64[] dyaw_pts +float64 dt +int32 n # equals to the pts size +--- +#result definition +float64 total_time +float64 total_distance_travelled +--- +#feedback +float64 remaining_time \ No newline at end of file diff --git a/trackers/kr_tracker_msgs/msg/Polynomial.msg b/trackers/kr_tracker_msgs/msg/Polynomial.msg new file mode 100644 index 00000000..602d21f6 --- /dev/null +++ b/trackers/kr_tracker_msgs/msg/Polynomial.msg @@ -0,0 +1,5 @@ +# There should be 1 more coeffs than the degree +int32 degree +float32 dt +uint8 basis +float32[] coeffs \ No newline at end of file diff --git a/trackers/kr_trackers/src/poly_tracker.cpp b/trackers/kr_trackers/src/poly_tracker.cpp new file mode 100644 index 00000000..4b210de1 --- /dev/null +++ b/trackers/kr_trackers/src/poly_tracker.cpp @@ -0,0 +1,638 @@ +#include "rclcpp_action/rclcpp_action.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_tracker_msgs/action/poly_tracker.hpp" +#include "kr_tracker_msgs/msg/tracker_status.hpp" +#include "kr_trackers/initial_conditions.hpp" +#include "kr_trackers/Tracker.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include +#include +#include +#include + +// traj data +struct TrajData +{ + /* info of generated traj */ + double traj_dur_ = 0, traj_yaw_dur_ = 0; + ros::Time start_time_; + int dim_; + + + traj_opt::Trajectory2D traj_2d_; + traj_opt::Trajectory3D traj_3d_; + traj_opt::Trajectory4D traj_with_yaw_; + traj_opt::Trajectory1D traj_yaw_; + bool has_solo_yaw_traj_ = false; + + traj_opt::DiscreteStates traj_discrete_; +}; + + +class PolyTracker : public kr_trackers_manager::Tracker +{ + public: + PolyTracker(void); + + void Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent); + bool Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd); + void Deactivate(void); + + kr_mav_msgs::msg::PositionCommand::ConstSharedPtr update(const nav_msgs::msg::Odometry::SharedPtr msg); + uint8_t status() const; + + private: + // action callbacks + 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); + + kr_mav_msgs::msg::PositionCommand position_cmd_; + + /*** odom related ***/ + double cur_yaw_, last_yaw_ = 0.0, last_yawdot_ = 0.0; + Eigen::Vector3d cur_pos_, last_pos_, last_goal_; + bool have_last_goal_ = false; + + using PolyTrackerAction = kr_tracker_msgs::action::PolyTracker; + using PolyTrackerGoalHandle = rclcpp_action::ServerGoalHandle; + rclcpp_action::Server::SharedPtr tracker_server_; + std::shared_ptr current_goal_handle_; + rclcpp::Logger logger_{rclcpp::get_logger("poly_tracker")}; + rclcpp::Clock::SharedPtr clock_; + std::recursive_mutex mutex_; + + bool pos_set_, goal_set_, goal_reached_, active_; + bool traj_set_ = false; + bool yaw_set_ = false; + + std::shared_ptr current_trajectory_, next_trajectory_; + + /*** yaw set up ***/ + // intial rotation + double init_final_yaw_, init_dyaw_, init_yaw_time_; + rclcpp::Time time_last_; + + + /*** parameters ***/ + double time_forward_ = 1.5; + double max_dyaw_ = 0.5 * M_PI; + double max_ddyaw_ = M_PI; + + std::pair calculate_yaw(Eigen::Vector3d &dir, double dt); + double range(double angle); + +}; + +PolyTracker::PolyTracker(void) : pos_set_(false), goal_set_(false), goal_reached_(false), active_(false) +{ +} + +void PolyTracker::Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) +{ + auto node = parent.lock(); + if(!node) return; + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + // Set up the action server. + tracker_server_ = rclcpp_action::create_server( + node, + "~/poly_tracker/PolyTracker", + std::bind(&PolyTracker::goal_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&PolyTracker::cancel_callback, this, std::placeholders::_1), + std::bind(&PolyTracker::handle_accepted_callback, this, std::placeholders::_1) + ); + + current_trajectory_.reset(new TrajData); + next_trajectory_.reset(new TrajData); + RCLCPP_WARN(logger_, "PolyTracker initialized!"); +} + +bool PolyTracker::Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) +{ + // Only allow activation if a goal has been set + if(pos_set_) + { + if(!current_goal_handle_ || !current_goal_handle_->is_active()) + { + RCLCPP_WARN(logger_, "TrajectoryTracker::Activate: goal_set_ is true but action server has no active goal - not activating."); + active_ = false; + return false; + } + active_ = true; + RCLCPP_WARN(logger_, "TrajectoryTracker::Activate: !"); + } + return active_; +} + +void PolyTracker::Deactivate(void) +{ + std::lock_guard lock(mutex_); + if(current_goal_handle_ && current_goal_handle_->is_active()) + { + RCLCPP_WARN(logger_, "PolyTracker::Deactivate: deactivated tracker while still tracking the goal."); + current_goal_handle_->abort(std::make_shared()); + current_goal_handle_.reset(); + } + + goal_set_ = false; + active_ = false; +} + +double PolyTracker::range(double angle) +{ + // range the angle into (-PI, PI] + double psi = angle; + while(psi > M_PI) + { + psi -= 2 * M_PI; + } + while(psi <= -M_PI) + { + psi += 2 * M_PI; + } + return psi; +} + +kr_mav_msgs::msg::PositionCommand::ConstSharedPtr PolyTracker::update(const nav_msgs::msg::Odometry::SharedPtr &msg) +{ + pos_set_ = true; + + cur_pos_(0) = msg->pose.pose.position.x; + cur_pos_(1) = msg->pose.pose.position.y; + cur_pos_(2) = msg->pose.pose.position.z; + cur_yaw_ = tf2::getYaw(msg->pose.pose.orientation); + + rclcpp::Time time_now = clock_->now(); + + if(!active_) + { + last_yaw_ = cur_yaw_; + last_goal_ = cur_pos_; + time_last_ = time_now; + last_pos_ = cur_pos_; + return kr_mav_msgs::PositionCommand::Ptr(); + } + + Eigen::Vector3d pos(Eigen::Vector3d::Zero()), vel(Eigen::Vector3d::Zero()), acc(Eigen::Vector3d::Zero()); + std::pair yaw_yawdot(last_yaw_, 0.0); + + Eigen::VectorXd wp, dwp, ddwp; // only + int cur_dim = 0; + if (next_trajectory_ != NULL) + { + cur_dim = next_trajectory_->dim_; + + } + else + { + cur_dim = current_trajectory_->dim_; + } + + wp.resize(cur_dim); + dwp.resize(cur_dim); + ddwp.resize(cur_dim); + + // safety + if(have_last_goal_ && (cur_pos_ - last_goal_).norm() <= 0.3) + { + pos = last_goal_; + } + else + { + pos = last_pos_; + } + + if(yaw_set_) // 1. rotate yaw mode + { + double dyaw = range(init_final_yaw_ - cur_yaw_); + + if(std::abs(dyaw) < 0.5 || init_yaw_time_ > 2.0) + { + yaw_set_ = false; + // ROS_INFO(" yaw_set finished "); + time_last_ = time_now; + return kr_mav_msgs::PositionCommand::Ptr(); + } + + double yaw_temp = cur_yaw_ + (time_now - time_last_).toSec() * init_dyaw_; + double desired_yaw = + init_final_yaw_ - cur_yaw_ >= 0 ? std::min(yaw_temp, init_final_yaw_) : std::max(yaw_temp, init_final_yaw_); + + yaw_yawdot.first = desired_yaw; + yaw_yawdot.second = init_dyaw_; + + init_yaw_time_ += (time_now - time_last_).seconds(); + } + else if(traj_set_) + { + + if(next_trajectory_ != NULL && (time_now - next_trajectory_->start_time_).seconds() >= 0.0) + { + current_trajectory_ = next_trajectory_; + next_trajectory_ = NULL; + } + + double t_cur = (time_now - current_trajectory_->start_time_).seconds(); + + if(t_cur < current_trajectory_->traj_dur_ && t_cur >= 0.0) + { + switch(current_trajectory_->dim_) + { + case 1: + { + + Eigen::VectorXd cur_state = current_trajectory_->traj_discrete_.getState(t_cur); + pos = cur_state.head(3); + vel = cur_state.segment(3, 3); + acc = cur_state.tail(3); + + /*** calculate yaw ***/ + Eigen::Vector3d dir = t_cur + time_forward_ <= current_trajectory_->traj_dur_ ? + current_trajectory_->traj_discrete_.getNextPos(t_cur + time_forward_) - pos : + current_trajectory_->traj_discrete_.getNextPos(current_trajectory_->traj_dur_) - pos; + yaw_yawdot = calculate_yaw(dir, (time_now - time_last_).toSec()); + + break; + } + case 2: + { + wp = current_trajectory_->traj_2d_.getPos(t_cur); + dwp = current_trajectory_->traj_2d_.getVel(t_cur); + pos.head(2) = wp; + pos(2) = last_goal_(2); + vel.head(2) = dwp; + break; + } + case 3: + { + pos = current_trajectory_->traj_3d_.getPos(t_cur); + vel = current_trajectory_->traj_3d_.getVel(t_cur); + acc = current_trajectory_->traj_3d_.getAcc(t_cur); + + if(current_trajectory_->has_solo_yaw_traj_) + { + yaw_yawdot.first = current_trajectory_->traj_yaw_.getPos(t_cur)(0); + yaw_yawdot.second = range(current_trajectory_->traj_yaw_.getVel(t_cur)(0)); + } + else + { + /*** calculate yaw ***/ + Eigen::Vector3d dir = t_cur + time_forward_ <= current_trajectory_->traj_dur_ ? + current_trajectory_->traj_3d_.getPos(t_cur + time_forward_) - pos : + current_trajectory_->traj_3d_.getPos(current_trajectory_->traj_dur_) - pos; + yaw_yawdot = calculate_yaw(dir, (time_now - time_last_).toSec()); + } + + break; + } + case 4: + { + wp = current_trajectory_->traj_with_yaw_.getPos(t_cur); + dwp = current_trajectory_->traj_with_yaw_.getVel(t_cur); + ddwp = current_trajectory_->traj_with_yaw_.getAcc(t_cur); + + pos = wp.head(3); + vel = dwp.head(3); + acc = ddwp.head(3); + + yaw_yawdot.first = wp(3); + yaw_yawdot.second = range(dwp(3)); + + + break; + } + } + last_pos_ = pos; + } + else + { + pos = last_pos_; + last_goal_ = pos; + have_last_goal_ = true; + // finish executing the trajectory + if(t_cur <= current_trajectory_->traj_yaw_dur_ && current_trajectory_->has_solo_yaw_traj_) + { + yaw_yawdot.first = current_trajectory_->traj_yaw_.getPos(t_cur)(0); + yaw_yawdot.second = range(current_trajectory_->traj_yaw_.getVel(t_cur)(0)); + } + else + { + yaw_yawdot.first = last_yaw_; + yaw_yawdot.second = 0.0; + traj_set_ = false; + } + } + + } + + // publish the command + position_cmd_.header.frame_id = msg->header.frame_id; + position_cmd_.header.stamp = time_now; + position_cmd_.position.x = pos(0); + position_cmd_.position.y = pos(1); + position_cmd_.position.z = pos(2); + position_cmd_.velocity.x = vel(0); + position_cmd_.velocity.y = vel(1); + position_cmd_.velocity.z = vel(2); + position_cmd_.acceleration.x = acc(0); + position_cmd_.acceleration.y = acc(1); + position_cmd_.acceleration.z = acc(2); + position_cmd_.yaw = yaw_yawdot.first; + position_cmd_.yaw_dot = yaw_yawdot.second; + + time_last_ = time_now; + last_yaw_ = yaw_yawdot.first; + + return std::make_shared(position_cmd_); +} + +rclcpp_action::GoalResponse PolyTracker::goal_callback(const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) +{ + (void)uuid; + std::lock_guard lock(mutex_); + // If another goal is already active, reject new goal unless we want to preempt + 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; + } + + // Accept all other goals + 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 + return rclcpp_action::CancelResponse::ACCEPT; +} + +void PolyTracker::handle_accepted_callback(const std::shared_ptr goal_handle) +{ + std::lock_guard lock(mutex_); + // 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() + auto goal = goal_handle->get_goal(); + + // Map goal fields to internal state similarly to original goal_callback logic + if(goal->set_yaw) + { + goal_set_ = true; + goal_reached_ = false; + init_final_yaw_ = goal->final_yaw; + init_dyaw_ = goal->final_yaw - cur_yaw_; + if(goal->final_yaw < 0 && std::abs(init_dyaw_ + 2 * M_PI) < std::abs(init_dyaw_)) + { + init_dyaw_ = init_dyaw_ + 2 * M_PI; + } + else if(cur_yaw_ < 0 && std::abs(init_dyaw_ - 2 * M_PI) < std::abs(init_dyaw_)) + { + init_dyaw_ = init_dyaw_ - 2 * M_PI; + } + init_dyaw_ = range(init_dyaw_); + if(init_dyaw_ > max_dyaw_) init_dyaw_ = max_dyaw_; + else if(init_dyaw_ < -max_dyaw_) init_dyaw_ = -max_dyaw_; + yaw_set_ = true; + init_yaw_time_ = 0.0; + } + else if(goal->seg_x.size() > 0 || goal->knots.size() > 0) + { + // continuous trajectory: reuse original parsing logic but with goal message + goal_set_ = true; + goal_reached_ = false; + double total_duration = 0.0; + double total_yaw_duration = 0.0; + std::vector> segs_1d; + std::vector> segs_2d; + std::vector> segs_3d; + std::vector> segs_4d; + next_trajectory_.reset(new TrajData); + + if(goal->cpts_status == 1) + { + RCLCPP_INFO(logger_, "PolyTracker: not implement now"); + return; + } + else if(goal->cpts_status == 2) + { + if(goal->yaw_pts.size() <= 0) + { + next_trajectory_->dim_ = 3; + } + else + { + next_trajectory_->dim_ = 4; + } + + size_t N = goal->pos_pts.size() - 1; + size_t M = goal->knots.size(); + size_t degree = M - N - 1; + + Eigen::MatrixXd pos_pts(N + 1, next_trajectory_->dim_); + Eigen::VectorXd knots(M); + for(size_t i = 0; i < M; ++i) knots(i) = goal->knots[i]; + for(size_t i = 0; i <= N; ++i) + { + pos_pts(i, 0) = goal->pos_pts[i].x; + pos_pts(i, 1) = goal->pos_pts[i].y; + pos_pts(i, 2) = goal->pos_pts[i].z; + } + + if(next_trajectory_->dim_ == 3) + { + for(size_t i = 0; i < M - 2 * degree; i++) + { + Eigen::MatrixXd cpts(degree + 1, next_trajectory_->dim_); + for(size_t j = 0; j <= degree; j++) cpts.row(j) = pos_pts.row(i + j); + double dt = knots(degree + i + 1) - knots(degree + i); + traj_opt::Piece<3> seg(traj_opt::BEZIER, cpts, dt, degree); + segs_3d.push_back(seg); + total_duration += dt; + } + } + if(next_trajectory_->dim_ == 4) + { + for(size_t i = 0; i < goal->yaw_pts.size(); ++i) pos_pts(i, 3) = goal->yaw_pts[i]; + for(size_t i = 0; i < M - 2 * degree; i++) + { + Eigen::MatrixXd cpts(degree + 1, next_trajectory_->dim_); + for(size_t j = 0; j <= degree; j++) cpts.row(j) = pos_pts.row(i + j); + double dt = knots(degree + i + 1) - knots(degree + i); + traj_opt::Piece<4> seg(traj_opt::BEZIER, cpts, dt, degree); + segs_4d.push_back(seg); + total_duration += dt; + } + } + } + else + { + size_t deg = goal->seg_x[0].degree; + if(goal->seg_z.size() <= 0) + { + next_trajectory_->dim_ = 2; + } + else if(goal->seg_yaw.size() <= 0) + { + next_trajectory_->dim_ = 3; + } + else if(goal->separate_yaw) + { + next_trajectory_->dim_ = 3; + next_trajectory_->has_solo_yaw_traj_ = true; + for(size_t i = 0; i < goal->seg_yaw.size(); ++i) + { + Eigen::MatrixXd Coeffs_yaw(1, deg + 1); + float dt = goal->seg_yaw[i].dt; + total_yaw_duration += dt; + for(size_t j = 0; j < deg + 1; ++j) Coeffs_yaw(0, j) = goal->seg_yaw[i].coeffs[j]; + traj_opt::Piece<1> seg(traj_opt::STANDARD, Coeffs_yaw, dt); + segs_1d.push_back(seg); + } + next_trajectory_->traj_yaw_ = traj_opt::Trajectory1D(segs_1d, total_yaw_duration); + next_trajectory_->traj_yaw_dur_ = total_yaw_duration; + } + + for(size_t i = 0; i < goal->seg_x.size(); ++i) + { + Eigen::MatrixXd Coeffs(next_trajectory_->dim_, deg + 1); + float dt = goal->seg_x[i].dt; + total_duration += dt; + for(size_t j = 0; j < deg + 1; ++j) + { + Coeffs(0, j) = goal->seg_x[i].coeffs[j]; + Coeffs(1, j) = goal->seg_y[i].coeffs[j]; + } + switch(next_trajectory_->dim_) + { + case 2: + { + traj_opt::Piece<2> seg(traj_opt::STANDARD, Coeffs, dt); + segs_2d.push_back(seg); + break; + } + case 3: + { + for(size_t j = 0; j < deg + 1; ++j) Coeffs(2, j) = goal->seg_z[i].coeffs[j]; + traj_opt::Piece<3> seg(traj_opt::STANDARD, Coeffs, dt); + segs_3d.push_back(seg); + break; + } + case 4: + { + for(size_t j = 0; j < deg + 1; ++j) + { + Coeffs(2, j) = goal->seg_z[i].coeffs[j]; + Coeffs(3, j) = goal->seg_yaw[i].coeffs[j]; + } + traj_opt::Piece<4> seg(traj_opt::STANDARD, Coeffs, dt); + segs_4d.push_back(seg); + break; + } + } + } + } + + next_trajectory_->start_time_ = goal->t_start; + next_trajectory_->traj_dur_ = total_duration; + switch(next_trajectory_->dim_) + { + case 2: + next_trajectory_->traj_2d_ = traj_opt::Trajectory2D(segs_2d, total_duration); + break; + case 3: + next_trajectory_->traj_3d_ = traj_opt::Trajectory3D(segs_3d, total_duration); + break; + case 4: + next_trajectory_->traj_with_yaw_ = traj_opt::Trajectory4D(segs_4d, total_duration); + break; + } + traj_set_ = true; + RCLCPP_WARN(logger_, "[Poly tracker] Duration: %f", total_duration); + RCLCPP_INFO(logger_, "PolyTracker: Set the poly trajectory"); + } + else if(goal->vel_pts.size() > 0) + { + double interval = goal->dt; + int Num = goal->n; + std::vector discrete_states; + for(size_t j = 0; j < static_cast(Num); ++j) + { + Eigen::VectorXd state(9); + state << goal->pos_pts[j].x, goal->pos_pts[j].y, goal->pos_pts[j].z, + goal->vel_pts[j].x, goal->vel_pts[j].y, goal->vel_pts[j].z, + goal->acc_pts[j].x, goal->acc_pts[j].y, goal->acc_pts[j].z; + discrete_states.push_back(state); + } + + next_trajectory_->traj_discrete_ = traj_opt::DiscreteStates(interval, Num, discrete_states); + next_trajectory_->dim_ = 1; + next_trajectory_->start_time_ = goal->t_start; + next_trajectory_->traj_dur_ = interval * (Num - 1); + traj_set_ = true; + RCLCPP_INFO(logger_, "PolyTracker: Set the discrete trajectory"); + } + else + { + RCLCPP_WARN(logger_, "PolyTracker: Invalid goal received! Ignoring"); + } +} + +///////// some helper functions +std::pair PolyTracker::calculate_yaw(Eigen::Vector3d &dir, double dt) +{ + std::pair yaw_yawdot(0, 0); + double yaw_temp = dir.norm() > 0.1 ? atan2(dir(1), dir(0)) : last_yaw_; + double yawdot = 0; + double d_yaw; + + d_yaw = range(yaw_temp - last_yaw_); + + + + const double YDM = d_yaw >= 0 ? max_dyaw_ : -max_dyaw_; + const double YDDM = d_yaw >= 0 ? max_ddyaw_ : -max_ddyaw_; + double d_yaw_max; + if(fabs(last_yawdot_ + dt * YDDM) <= fabs(YDM)) + { + d_yaw_max = last_yawdot_ * dt + 0.5 * YDDM * dt * dt; + } + else + { + double t1 = (YDM - last_yawdot_) / YDDM; + d_yaw_max = ((dt - t1) + dt) * (YDM - last_yawdot_) / 2.0; + } + + if(fabs(d_yaw) > fabs(d_yaw_max)) + { + d_yaw = d_yaw_max; + } + + yawdot = d_yaw / dt; + double yaw = last_yaw_ + d_yaw; + + yaw_yawdot.first = yaw; + yaw_yawdot.second = yawdot; + + last_yaw_ = yaw_yawdot.first; + last_yawdot_ = yaw_yawdot.second; + + return yaw_yawdot; +} + +uint8_t PolyTracker::status() const +{ + return tracker_server_->isActive() ? static_cast(kr_tracker_msgs::TrackerStatus::ACTIVE) : + static_cast(kr_tracker_msgs::TrackerStatus::SUCCEEDED); +} + +#include +PLUGINLIB_EXPORT_CLASS(PolyTracker, kr_trackers_manager::Tracker) \ No newline at end of file From a7b4d633ed5dab693b6efc63499db563fa3fa1ce Mon Sep 17 00:00:00 2001 From: Dexter Date: Wed, 15 Oct 2025 13:49:20 -0400 Subject: [PATCH 51/64] Fixed odom topics, updated vicon launch --- .../config/neurofly.yaml | 2 +- .../launch/control.launch.py | 35 +++++++++++-------- .../src/so3_control_component.cpp | 2 +- kr_mav_manager/src/manager.cpp | 10 ++++-- .../src/trackers_manager.cpp | 2 +- 5 files changed, 32 insertions(+), 19 deletions(-) diff --git a/interfaces/kr_betaflight_interface/config/neurofly.yaml b/interfaces/kr_betaflight_interface/config/neurofly.yaml index 8fccefb2..a601ff1f 100644 --- a/interfaces/kr_betaflight_interface/config/neurofly.yaml +++ b/interfaces/kr_betaflight_interface/config/neurofly.yaml @@ -3,7 +3,7 @@ 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.66 # [kg] +mass: 0.68 # [kg] disable_thrust_mapping: true use_body_rates: false # thrust_vs_rpm_cof_a_: 1.338e-06 # gf diff --git a/interfaces/kr_betaflight_interface/launch/control.launch.py b/interfaces/kr_betaflight_interface/launch/control.launch.py index e14f2954..583fa45e 100644 --- a/interfaces/kr_betaflight_interface/launch/control.launch.py +++ b/interfaces/kr_betaflight_interface/launch/control.launch.py @@ -36,23 +36,15 @@ def generate_launch_description(): # Define launch arguments vicon_args = [ # Adding mocap vicon specific parameters - DeclareLaunchArgument('mocap', default_value='false'), - DeclareLaunchArgument('mocap_server', default_value='mocap.perch'), + DeclareLaunchArgument('mocap', default_value='true'), + DeclareLaunchArgument('mocap_server', default_value='192.168.8.2'), DeclareLaunchArgument('mocap_frame_rate', default_value='100'), DeclareLaunchArgument('mocap_max_accel', default_value='10.0'), - ] - - # KR interface arguments - kr_args = [ - DeclareLaunchArgument('robot', default_value='neurofly1'), # set robot namespace - DeclareLaunchArgument('mass', default_value='.680'), # set mass AUW - DeclareLaunchArgument('odom', default_value='control_odom'), # set odom topic (vio/ukf/vicon) - DeclareLaunchArgument('so3_cmd', default_value='so3_cmd'), - ] + ] # ZED camera arguments zed_args = [ - DeclareLaunchArgument('zed_enable', default_value='true'), + DeclareLaunchArgument('zed_enable', default_value='false'), DeclareLaunchArgument('camera_name', default_value='zed'), DeclareLaunchArgument('camera_model', default_value='zedm'), DeclareLaunchArgument('publish_urdf', default_value='true'), @@ -65,6 +57,19 @@ def generate_launch_description(): DeclareLaunchArgument('publish_svo_clock', default_value='false'), ] + # KR interface arguments + kr_args = [ + DeclareLaunchArgument('robot', default_value='neurofly1'), # set robot namespace + DeclareLaunchArgument('mass', default_value='.680'), # set mass AUW + DeclareLaunchArgument( + 'odom', + default_value=PythonExpression([ + '"control_odom"' if LaunchConfiguration('zed_enable') == 'true' else '"odom"' + ]) + ), # set odom topic (vio/ukf/vicon) + DeclareLaunchArgument('so3_cmd', default_value='so3_cmd'), + ] + # Initialize launch description with all arguments ld = LaunchDescription(vicon_args + kr_args + zed_args) @@ -133,6 +138,7 @@ def generate_launch_description(): extra_arguments=[{'use_intra_process_comms': True}] ), ComposableNode( + condition=IfCondition(LaunchConfiguration('zed_enable')), package="quadrotor_ukf_ros2", plugin="QuadrotorUKFNode", name="quadrotor_ukf_ros2", @@ -181,6 +187,7 @@ def generate_launch_description(): package='mocap_vicon', executable='mocap_vicon_node', name='vicon', + namespace='vicon', output='screen', parameters=[ {'server_address': LaunchConfiguration('mocap_server')}, @@ -190,11 +197,11 @@ def generate_launch_description(): {'publish_pts': False}, {'fixed_frame_id': 'mocap'}, # Set to [''] to take in ALL models from Vicon - {'model_list': [LaunchConfiguration('robot')]}, + {'model_list': ['']}, ], remappings=[ # Uncomment and modify the remapping if needed - ('/neurofly1/odom', '/odom'), + ('/vicon/neurofly1/odom', '/neurofly1/odom'), ] ) ) diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index d4aa7fd3..5206feda 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -644,7 +644,7 @@ SO3ControlComponent::SO3ControlComponent(const rclcpp::NodeOptions &options) auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 2), qos_profile); odom_sub_ = this->create_subscription( - "control_odom", qos1, std::bind(&SO3ControlComponent::odom_callback, this, std::placeholders::_1)); + "odom", qos1, std::bind(&SO3ControlComponent::odom_callback, this, std::placeholders::_1)); position_cmd_sub_ = this->create_subscription( "trackers_manager/cmd", qos1, std::bind(&SO3ControlComponent::position_cmd_callback, this, std::placeholders::_1)); enable_motors_sub_ = this->create_subscription( diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index b48ec08d..d848107b 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -52,10 +52,16 @@ MAVManager::MAVManager() this->declare_parameter("mass", rclcpp::PARAMETER_DOUBLE); this->declare_parameter("odom_timeout", 0.1f); this->declare_parameter("takeoff_height", 0.5); - this->declare_parameter("robot", "cf3"); + this->declare_parameter("robot", "neurofly1"); std::string robot_name = this->get_parameter("robot").as_string(); RCLCPP_INFO(this->get_logger(), robot_name.c_str()); + + rclcpp::Time now = this->now(); + last_odom_t_ = now; + last_imu_t_ = now; + last_output_data_t_ = now; + last_heartbeat_t_ = now; // Action Client line_tracker_distance_client_ = @@ -115,7 +121,7 @@ MAVManager::MAVManager() // Subscribers odom_sub_ = - this->create_subscription("control_odom", qos, std::bind(&MAVManager::odometry_cb, this, _1)); + this->create_subscription("odom", qos, std::bind(&MAVManager::odometry_cb, this, _1)); heartbeat_sub_ = this->create_subscription("heartbeat", qos, std::bind(&MAVManager::heartbeat_cb, this, _1)); tracker_status_sub_ = this->create_subscription( diff --git a/trackers/kr_trackers_manager/src/trackers_manager.cpp b/trackers/kr_trackers_manager/src/trackers_manager.cpp index 544c9fb5..1d80383f 100644 --- a/trackers/kr_trackers_manager/src/trackers_manager.cpp +++ b/trackers/kr_trackers_manager/src/trackers_manager.cpp @@ -100,7 +100,7 @@ LifecycleCallbackReturn TrackersManager::on_configure(const rclcpp_lifecycle::St // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - sub_odom_ = this->create_subscription("control_odom", qos, std::bind(&TrackersManager::odom_callback, this, std::placeholders::_1)); + sub_odom_ = this->create_subscription("odom", qos, std::bind(&TrackersManager::odom_callback, this, std::placeholders::_1)); srv_tracker_ = this->create_service("~/transition", std::bind(&TrackersManager::transition_callback, this, std::placeholders::_1, std::placeholders::_2)); From dcb105b25e4411e391141f32699bfb3b2e3ce4c2 Mon Sep 17 00:00:00 2001 From: Dexter Date: Wed, 15 Oct 2025 14:47:26 -0400 Subject: [PATCH 52/64] Added bridge_armed_ mutex lock --- .../src/crsf/crsf_bridge.cpp | 21 +++++-------------- .../src/sbus/sbus_bridge.cpp | 9 ++++---- .../src/so3cmd_to_betaflight_component.cpp | 12 +++-------- 3 files changed, 13 insertions(+), 29 deletions(-) diff --git a/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp b/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp index ef6d29cc..cb30c7ab 100644 --- a/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp +++ b/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp @@ -90,21 +90,10 @@ void CrsfBridge::watchdogThread() { CrsfMsg kill_msg; kill_msg.setArmStateDisarmed(); sendCrsfMessageToSerialPort(kill_msg); - if (bridge_armed_) { + if (isBridgeArmed()) { disarmBridge(); } } - std::lock_guard battery_lock(battery_voltage_mutex_); - if ((time_now - time_last_battery_voltage_received_).seconds() > - kBatteryVoltageTimeout_) { - battery_voltage_ = 0.0; - if (perform_thrust_voltage_compensation_) { - RCLCPP_WARN_THROTTLE( - logger_, *node_->get_clock(), 1000, - "Can not perform battery voltage compensation because there " - "is no recent battery voltage measurement"); - } - } } } @@ -166,7 +155,7 @@ void CrsfBridge::handleReceivedCrsfMessage(const CrsfMsg &received_crsf_msg) // In case there are valid control commands, the bridge will stay in // AUTONOMOUS_FLIGHT, otherwise the watchdog will set the state to OFF RCLCPP_INFO(logger_, "Control authority returned by remote control."); - if (bridge_armed_) + if (isBridgeArmed()) { RCLCPP_INFO(logger_, "Bridge armed, setting bridge state to AUTONOMOUS_FLIGHT"); setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); @@ -207,9 +196,9 @@ void CrsfBridge::controlCommandCallback( return; } time_last_active_control_command_received_ = node_->now(); - if (!bridge_armed_ || bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) + if (!isBridgeArmed() || bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) { - if (!bridge_armed_ && msg->aux.enable_motors && + if (!isBridgeArmed() && msg->aux.enable_motors && bridge_state_ != BridgeState::RC_FLIGHT) { RCLCPP_WARN( @@ -221,7 +210,6 @@ void CrsfBridge::controlCommandCallback( } CrsfMsg crsf_msg_to_send; { - std::lock_guard battery_lock(battery_voltage_mutex_); crsf_msg_to_send = generateCrsfMessageFromSO3Command(msg, odom_q); } if (!msg->aux.enable_motors) @@ -487,6 +475,7 @@ void CrsfBridge::disarmBridge() bool CrsfBridge::isBridgeArmed() const { + std::lock_guard arm_lock(arm_mutex_); return bridge_armed_; } diff --git a/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp b/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp index 062ad5cc..01fb2f87 100644 --- a/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp +++ b/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp @@ -134,7 +134,7 @@ void SBusBridge::watchdogThread() kill_msg.setArmStateDisarmed(); sendSBusMessageToSerialPort(kill_msg); // disarm bridge if needed - if(bridge_armed_) + if(isBridgeArmed()) { disarmBridge(); } @@ -216,7 +216,7 @@ void SBusBridge::handleReceivedSbusMessage(const SBusMsg &received_sbus_msg) // In case there are valid control commands, the bridge will stay in // AUTONOMOUS_FLIGHT, otherwise the watchdog will set the state to OFF RCLCPP_INFO(logger_, "Control authority returned by remote control."); - if(bridge_armed_) + if(isBridgeArmed()) { RCLCPP_INFO(logger_, "Bridge armed, setting bridge state to AUTONOMOUS_FLIGHT"); setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); @@ -261,11 +261,11 @@ void SBusBridge::controlCommandCallback(const kr_mav_msgs::msg::SO3Command::Cons // may need more logic @TODO time_last_active_control_command_received_ = node_->now(); - if(!bridge_armed_ || bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) + if(!isBridgeArmed() || bridge_state_ != BridgeState::AUTONOMOUS_FLIGHT) { // If bridge is not armed we do not allow control commands to be sent // RC has priority over control commands for autonomous flying - if(!bridge_armed_ && msg->aux.enable_motors && bridge_state_ != BridgeState::RC_FLIGHT) + if(!isBridgeArmed() && msg->aux.enable_motors && bridge_state_ != BridgeState::RC_FLIGHT) { RCLCPP_WARN(logger_, "Received active control command but sbus bridge is not armed. " "Please arm the bridge before sending control commands."); @@ -582,6 +582,7 @@ void SBusBridge::disarmBridge() bool SBusBridge::isBridgeArmed() const { + std::lock_guard arm_lock(arm_mutex_); return bridge_armed_; } diff --git a/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp b/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp index ba1285e9..9423261b 100644 --- a/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp +++ b/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp @@ -35,9 +35,6 @@ class SO3CmdToBetaflight : public rclcpp::Node rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr imu_sub_; - // Motor status - int motor_status_; - // Timeout handling double so3_cmd_timeout_; rclcpp::Time last_so3_cmd_time_; @@ -51,8 +48,7 @@ class SO3CmdToBetaflight : public rclcpp::Node SO3CmdToBetaflight::SO3CmdToBetaflight(const rclcpp::NodeOptions &options) : Node("so3cmd_to_bridge", rclcpp::NodeOptions(options).use_intra_process_comms(true)), odom_set_(false), - so3_cmd_set_(false), - motor_status_(0) + so3_cmd_set_(false) { this->declare_parameter("so3_cmd_timeout", 0.25); this->declare_parameter("protocol_type", "crsf"); @@ -65,7 +61,7 @@ SO3CmdToBetaflight::SO3CmdToBetaflight(const rclcpp::NodeOptions &options) std::bind(&SO3CmdToBetaflight::so3_cmd_callback, this, std::placeholders::_1)); odom_sub_ = this->create_subscription( - "odom", 10, + "control_odom", 10, std::bind(&SO3CmdToBetaflight::odom_callback, this, std::placeholders::_1)); init_timer_ = this->create_wall_timer( @@ -97,7 +93,7 @@ void SO3CmdToBetaflight::so3_cmd_callback(const kr_mav_msgs::msg::SO3Command::Sh } // switch on motors - if (msg->aux.enable_motors && !bridge_->isBridgeArmed() && !motor_status_ && odom_set_) + if (msg->aux.enable_motors && !bridge_->isBridgeArmed() && odom_set_) { motors_on(); } @@ -152,13 +148,11 @@ void SO3CmdToBetaflight::motors_on() { if (bridge_) bridge_->armBridge(); - motor_status_ = 1; } void SO3CmdToBetaflight::motors_off() { if (bridge_) bridge_->disarmBridge(); - motor_status_ = 0; } #include From c724a81f0cace5fa1dc84a01f5e959fa897105ef Mon Sep 17 00:00:00 2001 From: Dexter Date: Wed, 15 Oct 2025 14:47:49 -0400 Subject: [PATCH 53/64] Switched back to control_odom --- .../kr_betaflight_interface/launch/control.launch.py | 8 ++++---- kr_mav_controllers/src/so3_control_component.cpp | 2 +- kr_mav_manager/src/manager.cpp | 2 +- trackers/kr_trackers_manager/src/trackers_manager.cpp | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/interfaces/kr_betaflight_interface/launch/control.launch.py b/interfaces/kr_betaflight_interface/launch/control.launch.py index 583fa45e..4a0da772 100644 --- a/interfaces/kr_betaflight_interface/launch/control.launch.py +++ b/interfaces/kr_betaflight_interface/launch/control.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): # Define launch arguments vicon_args = [ # Adding mocap vicon specific parameters - DeclareLaunchArgument('mocap', default_value='true'), + DeclareLaunchArgument('mocap', default_value='false'), DeclareLaunchArgument('mocap_server', default_value='192.168.8.2'), DeclareLaunchArgument('mocap_frame_rate', default_value='100'), DeclareLaunchArgument('mocap_max_accel', default_value='10.0'), @@ -44,7 +44,7 @@ def generate_launch_description(): # ZED camera arguments zed_args = [ - DeclareLaunchArgument('zed_enable', default_value='false'), + DeclareLaunchArgument('zed_enable', default_value='true'), DeclareLaunchArgument('camera_name', default_value='zed'), DeclareLaunchArgument('camera_model', default_value='zedm'), DeclareLaunchArgument('publish_urdf', default_value='true'), @@ -138,7 +138,7 @@ def generate_launch_description(): extra_arguments=[{'use_intra_process_comms': True}] ), ComposableNode( - condition=IfCondition(LaunchConfiguration('zed_enable')), + # condition=IfCondition(LaunchConfiguration('zed_enable')), package="quadrotor_ukf_ros2", plugin="QuadrotorUKFNode", name="quadrotor_ukf_ros2", @@ -201,7 +201,7 @@ def generate_launch_description(): ], remappings=[ # Uncomment and modify the remapping if needed - ('/vicon/neurofly1/odom', '/neurofly1/odom'), + ('/vicon/neurofly1/odom', '/neurofly1/control_odom'), ] ) ) diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index 5206feda..d4aa7fd3 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -644,7 +644,7 @@ SO3ControlComponent::SO3ControlComponent(const rclcpp::NodeOptions &options) auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 2), qos_profile); odom_sub_ = this->create_subscription( - "odom", qos1, std::bind(&SO3ControlComponent::odom_callback, this, std::placeholders::_1)); + "control_odom", qos1, std::bind(&SO3ControlComponent::odom_callback, this, std::placeholders::_1)); position_cmd_sub_ = this->create_subscription( "trackers_manager/cmd", qos1, std::bind(&SO3ControlComponent::position_cmd_callback, this, std::placeholders::_1)); enable_motors_sub_ = this->create_subscription( diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index d848107b..046e2234 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -121,7 +121,7 @@ MAVManager::MAVManager() // Subscribers odom_sub_ = - this->create_subscription("odom", qos, std::bind(&MAVManager::odometry_cb, this, _1)); + this->create_subscription("control_odom", qos, std::bind(&MAVManager::odometry_cb, this, _1)); heartbeat_sub_ = this->create_subscription("heartbeat", qos, std::bind(&MAVManager::heartbeat_cb, this, _1)); tracker_status_sub_ = this->create_subscription( diff --git a/trackers/kr_trackers_manager/src/trackers_manager.cpp b/trackers/kr_trackers_manager/src/trackers_manager.cpp index 1d80383f..544c9fb5 100644 --- a/trackers/kr_trackers_manager/src/trackers_manager.cpp +++ b/trackers/kr_trackers_manager/src/trackers_manager.cpp @@ -100,7 +100,7 @@ LifecycleCallbackReturn TrackersManager::on_configure(const rclcpp_lifecycle::St // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - sub_odom_ = this->create_subscription("odom", qos, std::bind(&TrackersManager::odom_callback, this, std::placeholders::_1)); + sub_odom_ = this->create_subscription("control_odom", qos, std::bind(&TrackersManager::odom_callback, this, std::placeholders::_1)); srv_tracker_ = this->create_service("~/transition", std::bind(&TrackersManager::transition_callback, this, std::placeholders::_1, std::placeholders::_2)); From 991930825f4509ef1416ab7c7562e7482113f5b5 Mon Sep 17 00:00:00 2001 From: Dexter Date: Wed, 15 Oct 2025 19:04:19 -0400 Subject: [PATCH 54/64] Updated gains --- .../kr_betaflight_interface/config/gains.yaml | 20 +++++++++---------- .../config/tracker_params.yaml | 12 +++++------ .../config/trackers.yaml | 10 +++++----- .../launch/control.launch.py | 6 +++--- 4 files changed, 24 insertions(+), 24 deletions(-) diff --git a/interfaces/kr_betaflight_interface/config/gains.yaml b/interfaces/kr_betaflight_interface/config/gains.yaml index 8903bad7..6114c865 100644 --- a/interfaces/kr_betaflight_interface/config/gains.yaml +++ b/interfaces/kr_betaflight_interface/config/gains.yaml @@ -2,12 +2,12 @@ quadrotor_name: neurofly1 mass: 0.68 -kp_x: 3.4 -kp_y: 3.4 -kp_z: 5.4 -kd_x: 2.8 -kd_y: 2.8 -kd_z: 3.0 +kp_x: 6.0 +kp_y: 6.0 +kp_z: 8.0 +kd_x: 3.0 +kd_y: 3.0 +kd_z: 3.5 ki_x: 0.00 ki_y: 0.00 ki_z: 0.00 @@ -16,8 +16,8 @@ kib_x: 0.00 kib_y: 0.00 kib_z: 0.00 -rot_x: 1.5 -rot_y: 1.5 +rot_x: 1.2 +rot_y: 1.2 rot_z: 1.0 ang_x: 0.13 @@ -28,5 +28,5 @@ kf_correction: 0.0e-08 roll_correction: 0.0 pitch_correction: 0.0 -max_pos_int: 0.5 -max_pos_int_b: 0.5 \ No newline at end of file +max_pos_int: 0.3 +max_pos_int_b: 0.3 \ No newline at end of file diff --git a/interfaces/kr_betaflight_interface/config/tracker_params.yaml b/interfaces/kr_betaflight_interface/config/tracker_params.yaml index 64c0d4b9..af1d71a1 100644 --- a/interfaces/kr_betaflight_interface/config/tracker_params.yaml +++ b/interfaces/kr_betaflight_interface/config/tracker_params.yaml @@ -1,19 +1,19 @@ # This should contain tracker parameters line_tracker_distance: - default_v_des: 1.0 - default_a_des: 0.5 + 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.8 - default_yaw_a_des: 0.2 + default_yaw_v_des: 0.4 + default_yaw_a_des: 0.3 trajectory_tracker: - max_vel_des: 0.5 - max_acc_des: 0.3 + max_vel_des: 2.0 + max_acc_des: 2.0 velocity_tracker: timeout: 0.5 diff --git a/interfaces/kr_betaflight_interface/config/trackers.yaml b/interfaces/kr_betaflight_interface/config/trackers.yaml index 8d48c1d7..f06c94fb 100644 --- a/interfaces/kr_betaflight_interface/config/trackers.yaml +++ b/interfaces/kr_betaflight_interface/config/trackers.yaml @@ -1,10 +1,10 @@ /neurofly1/trackers_manager: ros__parameters: trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk"] - line_tracker_distance/default_v_des: 1.0 - line_tracker_distance/default_a_des: 0.5 + 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.3 - line_tracker_min_jerk/default_yaw_v_des: 0.8 - line_tracker_min_jerk/default_yaw_a_des: 0.2 + 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 diff --git a/interfaces/kr_betaflight_interface/launch/control.launch.py b/interfaces/kr_betaflight_interface/launch/control.launch.py index 4a0da772..d1108bc8 100644 --- a/interfaces/kr_betaflight_interface/launch/control.launch.py +++ b/interfaces/kr_betaflight_interface/launch/control.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): # Define launch arguments vicon_args = [ # Adding mocap vicon specific parameters - DeclareLaunchArgument('mocap', default_value='false'), + DeclareLaunchArgument('mocap', default_value='true'), DeclareLaunchArgument('mocap_server', default_value='192.168.8.2'), DeclareLaunchArgument('mocap_frame_rate', default_value='100'), DeclareLaunchArgument('mocap_max_accel', default_value='10.0'), @@ -44,7 +44,7 @@ def generate_launch_description(): # ZED camera arguments zed_args = [ - DeclareLaunchArgument('zed_enable', default_value='true'), + DeclareLaunchArgument('zed_enable', default_value='false'), DeclareLaunchArgument('camera_name', default_value='zed'), DeclareLaunchArgument('camera_model', default_value='zedm'), DeclareLaunchArgument('publish_urdf', default_value='true'), @@ -138,7 +138,7 @@ def generate_launch_description(): extra_arguments=[{'use_intra_process_comms': True}] ), ComposableNode( - # condition=IfCondition(LaunchConfiguration('zed_enable')), + condition=IfCondition(LaunchConfiguration('zed_enable')), package="quadrotor_ukf_ros2", plugin="QuadrotorUKFNode", name="quadrotor_ukf_ros2", From 50f7a000c94b1045fb5543df282e3600eb0aae3b Mon Sep 17 00:00:00 2001 From: Dexter Date: Wed, 15 Oct 2025 19:09:46 -0400 Subject: [PATCH 55/64] Renamed gains file --- .../config/{gains.yaml => neurofly_gains.yaml} | 0 interfaces/kr_betaflight_interface/launch/control.launch.py | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename interfaces/kr_betaflight_interface/config/{gains.yaml => neurofly_gains.yaml} (100%) diff --git a/interfaces/kr_betaflight_interface/config/gains.yaml b/interfaces/kr_betaflight_interface/config/neurofly_gains.yaml similarity index 100% rename from interfaces/kr_betaflight_interface/config/gains.yaml rename to interfaces/kr_betaflight_interface/config/neurofly_gains.yaml diff --git a/interfaces/kr_betaflight_interface/launch/control.launch.py b/interfaces/kr_betaflight_interface/launch/control.launch.py index d1108bc8..484b2ec3 100644 --- a/interfaces/kr_betaflight_interface/launch/control.launch.py +++ b/interfaces/kr_betaflight_interface/launch/control.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): so3_config_file = os.path.join( get_package_share_directory('kr_betaflight_interface'), 'config', - 'gains.yaml' + 'neurofly_gains.yaml' ) # URDF/xacro file to be loaded by the Robot State Publisher node From 3aee9955f94329089e01359d218c6ef58c4fdce1 Mon Sep 17 00:00:00 2001 From: Dexter Date: Thu, 23 Oct 2025 07:02:21 -0400 Subject: [PATCH 56/64] Updates to polytracker --- .../config/trackers.yaml | 2 +- .../src/so3_control_component.cpp | 7 + trackers/kr_trackers/CMakeLists.txt | 2 + trackers/kr_trackers/plugins_description.xml | 5 + .../src/line_tracker_distance_server.cpp | 11 + .../src/line_tracker_min_jerk_server.cpp | 14 +- trackers/kr_trackers/src/poly_tracker.cpp | 30 +- trackers/kr_trackers/utils/traj_data.hpp | 393 ++++++++++++++++++ .../config/trackers_manager.yaml | 2 +- 9 files changed, 449 insertions(+), 17 deletions(-) create mode 100644 trackers/kr_trackers/utils/traj_data.hpp diff --git a/interfaces/kr_betaflight_interface/config/trackers.yaml b/interfaces/kr_betaflight_interface/config/trackers.yaml index f06c94fb..f8d96393 100644 --- a/interfaces/kr_betaflight_interface/config/trackers.yaml +++ b/interfaces/kr_betaflight_interface/config/trackers.yaml @@ -1,6 +1,6 @@ /neurofly1/trackers_manager: ros__parameters: - trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk"] + trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk", "PolyTracker"] line_tracker_distance/default_v_des: 0.2 line_tracker_distance/default_a_des: 0.2 line_tracker_distance/epsilon: 0.1 diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index d4aa7fd3..f16a9ef6 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -69,6 +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_); + + // print 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); @@ -76,6 +81,8 @@ void SO3ControlComponent::publishSO3Command() 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)); + auto so3_command = std::make_unique(); so3_command->header.stamp = this->now(); so3_command->header.frame_id = frame_id_; diff --git a/trackers/kr_trackers/CMakeLists.txt b/trackers/kr_trackers/CMakeLists.txt index e23f11fd..b46f40b8 100644 --- a/trackers/kr_trackers/CMakeLists.txt +++ b/trackers/kr_trackers/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(pluginlib REQUIRED) include_directories( include + utils ) set(plugins_dependencies @@ -46,6 +47,7 @@ add_library(${PROJECT_NAME}_plugins SHARED src/line_tracker_distance_server.cpp src/line_tracker_min_jerk_server.cpp src/null_tracker.cpp + src/poly_tracker.cpp src/initial_conditions.cpp) ament_target_dependencies(${PROJECT_NAME}_plugins ${node_dependencies}) diff --git a/trackers/kr_trackers/plugins_description.xml b/trackers/kr_trackers/plugins_description.xml index 73dab00d..de0a74d2 100644 --- a/trackers/kr_trackers/plugins_description.xml +++ b/trackers/kr_trackers/plugins_description.xml @@ -16,5 +16,10 @@ This is a tracker which follows a line from the current position to a given goal position using min jerk trajectory. + + Action-based poly tracker implementation + + diff --git a/trackers/kr_trackers/src/line_tracker_distance_server.cpp b/trackers/kr_trackers/src/line_tracker_distance_server.cpp index 8f5ae5ae..9f194f76 100644 --- a/trackers/kr_trackers/src/line_tracker_distance_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_distance_server.cpp @@ -222,6 +222,17 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerDistance::update(co cmd->velocity.x = 0, cmd->velocity.y = 0, cmd->velocity.z = 0; cmd->acceleration.x = 0, cmd->acceleration.y = 0, cmd->acceleration.z = 0; + // patch to track yaw while at goal + const float target_yaw = start_yaw_; + const float yaw_error = std::atan2(std::sin(target_yaw - yaw_), std::cos(target_yaw - yaw_)); + const float max_yaw_rate = 0.3f; + const float kp_yaw = 1.0f; // TODO make config + + float yaw_rate = kp_yaw * yaw_error; + yaw_rate = std::max(-max_yaw_rate, std::min(max_yaw_rate, yaw_rate)); //clamp + cmd->yaw = target_yaw; + cmd->yaw_dot = yaw_rate; + ICs_.set_from_cmd(cmd); return cmd; } 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 dd5fd95c..ca266af0 100644 --- a/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp @@ -328,11 +328,21 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LineTrackerMinJerk::update(con } cmd->position.x = goal_(0), cmd->position.y = goal_(1), cmd->position.z = goal_(2); - cmd->yaw = goal_yaw_; - cmd->yaw_dot = 0; cmd->velocity.x = 0, cmd->velocity.y = 0, cmd->velocity.z = 0; cmd->acceleration.x = 0, cmd->acceleration.y = 0, cmd->acceleration.z = 0; + // patch to track yaw while at goal + const float current_yaw = tf2::getYaw(msg->pose.pose.orientation); + const float target_yaw = goal_yaw_; + const float yaw_error = std::atan2(std::sin(target_yaw - current_yaw), std::cos(target_yaw - current_yaw)); + const float max_yaw_rate = 0.3f; + const float kp_yaw = 1.0f; // TODO make config + + float yaw_rate = kp_yaw * yaw_error; + yaw_rate = std::max(-max_yaw_rate, std::min(max_yaw_rate, yaw_rate)); // clamp + cmd->yaw = target_yaw; + cmd->yaw_dot = yaw_rate; + ICs_.set_from_cmd(cmd); return cmd; } diff --git a/trackers/kr_trackers/src/poly_tracker.cpp b/trackers/kr_trackers/src/poly_tracker.cpp index 4b210de1..ee507d7e 100644 --- a/trackers/kr_trackers/src/poly_tracker.cpp +++ b/trackers/kr_trackers/src/poly_tracker.cpp @@ -18,7 +18,7 @@ struct TrajData { /* info of generated traj */ double traj_dur_ = 0, traj_yaw_dur_ = 0; - ros::Time start_time_; + rclcpp::Time start_time_; int dim_; @@ -42,7 +42,7 @@ class PolyTracker : public kr_trackers_manager::Tracker void Deactivate(void); kr_mav_msgs::msg::PositionCommand::ConstSharedPtr update(const nav_msgs::msg::Odometry::SharedPtr msg); - uint8_t status() const; + uint8_t status(); private: // action callbacks @@ -158,7 +158,7 @@ double PolyTracker::range(double angle) return psi; } -kr_mav_msgs::msg::PositionCommand::ConstSharedPtr PolyTracker::update(const nav_msgs::msg::Odometry::SharedPtr &msg) +kr_mav_msgs::msg::PositionCommand::ConstSharedPtr PolyTracker::update(const nav_msgs::msg::Odometry::SharedPtr msg) { pos_set_ = true; @@ -175,7 +175,7 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr PolyTracker::update(const nav_ last_goal_ = cur_pos_; time_last_ = time_now; last_pos_ = cur_pos_; - return kr_mav_msgs::PositionCommand::Ptr(); + return std::make_shared(position_cmd_); } Eigen::Vector3d pos(Eigen::Vector3d::Zero()), vel(Eigen::Vector3d::Zero()), acc(Eigen::Vector3d::Zero()); @@ -216,10 +216,10 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr PolyTracker::update(const nav_ yaw_set_ = false; // ROS_INFO(" yaw_set finished "); time_last_ = time_now; - return kr_mav_msgs::PositionCommand::Ptr(); + return std::make_shared(position_cmd_); } - double yaw_temp = cur_yaw_ + (time_now - time_last_).toSec() * init_dyaw_; + double yaw_temp = cur_yaw_ + (time_now - time_last_).seconds() * init_dyaw_; double desired_yaw = init_final_yaw_ - cur_yaw_ >= 0 ? std::min(yaw_temp, init_final_yaw_) : std::max(yaw_temp, init_final_yaw_); @@ -255,7 +255,7 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr PolyTracker::update(const nav_ Eigen::Vector3d dir = t_cur + time_forward_ <= current_trajectory_->traj_dur_ ? current_trajectory_->traj_discrete_.getNextPos(t_cur + time_forward_) - pos : current_trajectory_->traj_discrete_.getNextPos(current_trajectory_->traj_dur_) - pos; - yaw_yawdot = calculate_yaw(dir, (time_now - time_last_).toSec()); + yaw_yawdot = calculate_yaw(dir, (time_now - time_last_).seconds()); break; } @@ -285,7 +285,7 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr PolyTracker::update(const nav_ Eigen::Vector3d dir = t_cur + time_forward_ <= current_trajectory_->traj_dur_ ? current_trajectory_->traj_3d_.getPos(t_cur + time_forward_) - pos : current_trajectory_->traj_3d_.getPos(current_trajectory_->traj_dur_) - pos; - yaw_yawdot = calculate_yaw(dir, (time_now - time_last_).toSec()); + yaw_yawdot = calculate_yaw(dir, (time_now - time_last_).seconds()); } break; @@ -628,11 +628,15 @@ std::pair PolyTracker::calculate_yaw(Eigen::Vector3d &dir, doubl return yaw_yawdot; } -uint8_t PolyTracker::status() const +uint8_t PolyTracker::status() { - return tracker_server_->isActive() ? static_cast(kr_tracker_msgs::TrackerStatus::ACTIVE) : - static_cast(kr_tracker_msgs::TrackerStatus::SUCCEEDED); + // prefer checking current goal handle if available (other trackers use this pattern) + if(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); } -#include -PLUGINLIB_EXPORT_CLASS(PolyTracker, kr_trackers_manager::Tracker) \ No newline at end of file +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(PolyTracker, kr_trackers_manager::Tracker); \ No newline at end of file diff --git a/trackers/kr_trackers/utils/traj_data.hpp b/trackers/kr_trackers/utils/traj_data.hpp new file mode 100644 index 00000000..d383ea64 --- /dev/null +++ b/trackers/kr_trackers/utils/traj_data.hpp @@ -0,0 +1,393 @@ +// Copyright 2015 Michael Watterson, University of Pennsylvania +// Revised 2022 by Yuwei +#ifndef TRAJ_DATcoeffsHPP +#define TRAJ_DATcoeffsHPP + +#include +#include +#include +#include + +namespace traj_opt +{ + +enum PolyType +{ + STANDARD, // polynomials + BEZIER, // B-spline and bernstein + BERNSTEIN, + ENDPOINT, + CHEBYSHEV +}; + +/** + * @brief + * + * - STANDARD: polynomial trajectory + * + * p(t) = C * [1, t, t^2, t^3, t^4, t^5]^T + * + * polynomials are stored with parameterization s \in [0,1] + * time dt dt is used to evaluate polynomial p(t/dt) for t \in [0,dt] + * + * - BEZIER: + * - BERNSTEIN: + * + * p(t) = cpts * A * [1, t, t^2, t^3, t^4, t^5]^T + * cpts: control points (N+1)x3 + * A: transformation matrix (N+1) * (N+1) + * + * then C = cpts * A can also represent with general matrix representations + * + * reference: + * Kaihuai Qin, "General matrix representations for B-splines," + * Proceedings Pacific Graphics '98. Sixth Pacific Conference on Computer Graphics and Applications (Cat. No.98EX208), + * 1998, pp. 37-43, doi: 10.1109/PCCGA.1998.731996. + * + **/ + +// one segment of a trajectory, one dimension +template +class Piece +{ + private: + int degree = 5; // set 5 + float dt; // dt + PolyType basis; + Eigen::MatrixXd ncoeffs; + + public: + + Piece(){} + ~Piece(){} + + Piece(PolyType ptype, Eigen::MatrixXd ncoeffs_matrix, float dur) + { + basis = ptype; + ncoeffs = ncoeffs_matrix; + dt = dur; + degree = ncoeffs.cols() -1; + } + + Piece(PolyType ptype, Eigen::MatrixXd cpts, float dur, int deg) + { + basis = ptype; + degree = deg; + dt = dur; + getNcoeffs(cpts); + } + + inline double getDur() const{ return dt;} + + inline Eigen::VectorXd getPos(double t) + { + // Normalize the time + t /= dt; + Eigen::VectorXd pos(dim); + pos.setZero(); + double tn = 1.0; + for (int i = 0; i <= degree; i++) + { + pos += tn * ncoeffs.col(i); + tn *= t; + } + return pos; + } + + // Get the velocity at time t in this piece + inline Eigen::VectorXd getVel(double t) + { + t /= dt; + Eigen::VectorXd vel(dim); + vel.setZero(); + double tn = 1.0; + int n = 1; + for (int i = 1; i <= degree; i++) + { + vel += n * tn * ncoeffs.col(i); + tn *= t; + n++; + } + vel /= dt; + return vel; + } + + // Get the acceleration at time t in this piece + inline Eigen::VectorXd getAcc(double t) + { + t /= dt; + Eigen::VectorXd acc(dim); + acc.setZero(); + double tn = 1.0; + int m = 1; + int n = 2; + for (int i = 2; i <= degree; i++) + { + acc += m * n * tn * ncoeffs.col(i); + tn *= t; + m++; + n++; + } + acc /= dt * dt; + return acc; + } + + + private: + + void getNcoeffs(Eigen::MatrixXd& cpts) + { + Eigen::MatrixXd A(degree + 1, degree + 1); + A.setZero(); + switch (basis) + { + case BEZIER: + { + switch (degree) + { + case 1: + A << 1, -1, + 0, 1; + break; + case 2: + { + A << 1, -2, 1, + 1, 2, -2, + 0, 0, 1; + A /= 2.0; + break; + } + case 3: + { + A << 1, -3, 3, 1, + 4, 0, -6, 3, + 1, 3, 3, -3, + 0, 0, 0, 1; + A /= 6.0; + break; + } + case 4: + { + A << 1, -4, 6, -4, 1, + 11,-12, -6, 12, -4, + 11, 12, -6, -12, 6, + 1, 4, 6, 4, -4, + 0, 0, 0, 0, 1; + A /= 24.0; + break; + } + } + break; + } + case BERNSTEIN: + { + switch (degree) + { + case 1: + A << 1, -1, + 0, 1; + break; + case 2: + A << 1, -2, 1, + 0, 2, -2, + 0, 0, 1; + break; + case 3: + A << 1, -3, 3, 1, + 0, 3, -6, 3, + 0, 0, 3, -3, + 0, 0, 0, 1; + break; + case 4: + A << 1, -4, 6, -4, 1, + 0, 4, -12, 12, -4, + 0, 0, 6, -12, 6, + 0, 0, 0, 4, -4, + 0, 0, 0, 0, 1; + break; + } + break; + } + } + ncoeffs = cpts.transpose() * A; + + } + +}; + + +/** + * @brief trajectory data + * @tparam dim = 1-4 + */ +template +class Trajectory +{ + private: + std::vector> seg_pieces; + int seg_num; + double dttotal; + + public: + Trajectory(){} + ~Trajectory(){} + + Trajectory(std::vector> segs, double dur) + { + seg_pieces = segs; + dttotal = dur; + seg_num = seg_pieces.size(); + } + + inline bool isValid() const + { + if(seg_num <= 0){ return false;} + return true; + } + + // Find the piece at which the time t is located + // The index is returned and the offset in t is removed + inline int locatePieceIdx(double &t) const + { + int idx; + double dur; + for(idx = 0; idx < seg_num && t > (dur = seg_pieces[idx].getDur()); idx++) + { + t -= dur; + } + if(idx == seg_num) + { + idx--; + t += seg_pieces[idx].getDur(); + } + return idx; + } + + inline Eigen::VectorXd getPos(double t) + { + int pieceIdx = locatePieceIdx(t); + return seg_pieces[pieceIdx].getPos(t); + } + + inline Eigen::VectorXd getVel(double t) + { + int pieceIdx = locatePieceIdx(t); + return seg_pieces[pieceIdx].getVel(t); + } + + inline Eigen::VectorXd getAcc(double t) + { + int pieceIdx = locatePieceIdx(t); + return seg_pieces[pieceIdx].getAcc(t); + } +}; + +typedef Trajectory<1> Trajectory1D; +/// Trajectory in 2D +typedef Trajectory<2> Trajectory2D; +/// Trajectory in 3D +typedef Trajectory<3> Trajectory3D; +/// Trajectory in 3D with yaw +typedef Trajectory<4> Trajectory4D; + + +class DiscreteStates +{ + private: + int N = 20; //num of states + double dt; + + std::vector states; // p v a + bool is_linear_cut = false; + + public: + + DiscreteStates(){} + DiscreteStates(const double interval, + const int num, + std::vector &discrete_states) + : dt(interval), N(num), states(discrete_states) + { + + } + ~DiscreteStates() = default; + + inline void useLinearCut() + { + is_linear_cut = true; + } + + inline Eigen::VectorXd getState(double t) + { + + int index = std::floor(t / dt); + + Eigen::VectorXd x1 = states[index]; + Eigen::VectorXd x2 = states[index+1]; + + double tau = t - index * dt; + + //std::cout << " index " << index << std::endl; + + if(is_linear_cut) + { + return x1 + tau / dt * (x2 - x1); + } + + Eigen::MatrixXd psi = (Phi(dt - tau).transpose()); + Eigen::MatrixXd lambda = Phi(tau) - psi * Phi(dt); + + return lambda * x1 + psi * x2; + } + + + + inline Eigen::VectorXd getPreState(double t) + { + + int index = std::floor(t / dt); + + return states[std::min(index, N-1)]; + } + + + inline Eigen::VectorXd getNextState(double t) + { + + int index = std::floor(t / dt); + + return states[std::min(index+1, N-1)]; + } + + + inline Eigen::Vector3d getNextPos(double t) + { + + int index = std::floor(t / dt); + + return states[std::min(index+1, N-1)].head(3); + } + + + + private: + + //state transit matrix 9 * 9 + static inline Eigen::MatrixXd Phi(const double tau) + { + Eigen::MatrixXd phi = Eigen::Matrix::Identity(); + + for (int i = 0; i < 6; ++i) + phi(i, i + 3) = tau; + + for (int i = 0; i < 3; ++i) + phi(i, i + 6) = 0.5 * tau * tau; + return phi; + } + +}; + + +} // namespace traj_opt + +#endif \ No newline at end of file diff --git a/trackers/kr_trackers_manager/config/trackers_manager.yaml b/trackers/kr_trackers_manager/config/trackers_manager.yaml index 22d73b55..db36a54f 100644 --- a/trackers/kr_trackers_manager/config/trackers_manager.yaml +++ b/trackers/kr_trackers_manager/config/trackers_manager.yaml @@ -1,6 +1,6 @@ trackers_manager: ros__parameters: - trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk"] + trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk", "PolyTracker"] line_tracker_distance/default_v_des: 0.5 line_tracker_distance/default_a_des: 0.5 line_tracker_distance/epsilon: 0.1 From 200b21cfb7307180e6d09a1562e647aa2622ab5d Mon Sep 17 00:00:00 2001 From: Dexter Date: Thu, 23 Oct 2025 10:38:14 -0400 Subject: [PATCH 57/64] Added polytracker services --- .../include/kr_mav_manager/manager.hpp | 10 +++++ .../kr_mav_manager/mav_manager_services.hpp | 41 ++++++++++++++++++ kr_mav_manager/src/manager.cpp | 43 +++++++++++++++++++ trackers/kr_trackers/src/poly_tracker.cpp | 8 ++-- 4 files changed, 98 insertions(+), 4 deletions(-) diff --git a/kr_mav_manager/include/kr_mav_manager/manager.hpp b/kr_mav_manager/include/kr_mav_manager/manager.hpp index 6afd6b72..2551678e 100644 --- a/kr_mav_manager/include/kr_mav_manager/manager.hpp +++ b/kr_mav_manager/include/kr_mav_manager/manager.hpp @@ -23,6 +23,7 @@ #include "kr_tracker_msgs/action/line_tracker.hpp" #include "kr_tracker_msgs/action/lissajous_adder.hpp" #include "kr_tracker_msgs/action/lissajous_tracker.hpp" +#include "kr_tracker_msgs/action/poly_tracker.hpp" #include "kr_tracker_msgs/msg/tracker_status.hpp" #include "kr_tracker_msgs/msg/velocity_goal.hpp" #include "kr_tracker_msgs/srv/transition.hpp" @@ -39,6 +40,9 @@ using LissajousTrackerGoalHandle = rclcpp_action::ClientGoalHandle; +using PolyTracker = kr_tracker_msgs::action::PolyTracker; +using PolyTrackerGoalHandle = rclcpp_action::ClientGoalHandle; + class MAVManager : public rclcpp::Node { public: @@ -112,6 +116,9 @@ class MAVManager : public rclcpp::Node float y_num_periods[2], float z_num_periods[2], float yaw_num_periods[2], float period[2], float num_cycles[2], float ramp_time[2]); + // PolyTracker + bool sendPolyGoal(const PolyTracker::Goal &goal); + // Direct low-level control bool setPositionCommand(const kr_mav_msgs::msg::PositionCommand &msg); bool setSO3Command(const kr_mav_msgs::msg::SO3Command &msg); @@ -147,11 +154,13 @@ class MAVManager : public rclcpp::Node typedef rclcpp_action::Client::SharedPtr CircleClientType; typedef rclcpp_action::Client::SharedPtr LissajousClientType; typedef rclcpp_action::Client::SharedPtr CompoundLissajousClientType; + typedef rclcpp_action::Client::SharedPtr PolyClientType; void tracker_done_callback(const LineTrackerGoalHandle::WrappedResult &result); void circle_tracker_done_callback(const CircleTrackerGoalHandle::WrappedResult &result); void lissajous_tracker_done_callback(const LissajousTrackerGoalHandle::WrappedResult &result); void lissajous_adder_done_callback(const LissajousAdderGoalHandle::WrappedResult &result); + void poly_tracker_done_callback(const PolyTrackerGoalHandle::WrappedResult &result); void odometry_cb(nav_msgs::msg::Odometry::ConstSharedPtr msg); void imu_cb(sensor_msgs::msg::Imu::ConstSharedPtr msg); @@ -189,6 +198,7 @@ class MAVManager : public rclcpp::Node CircleClientType circle_tracker_client_; LissajousClientType lissajous_tracker_client_; CompoundLissajousClientType lissajous_adder_client_; + PolyClientType poly_tracker_client_; // Publishers rclcpp::Publisher::SharedPtr pub_motors_; 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 3d095264..633fcb70 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 @@ -8,6 +8,7 @@ #include "kr_mav_manager/srv/vec4.hpp" #include "std_srvs/srv/set_bool.hpp" #include "std_srvs/srv/trigger.hpp" +#include "kr_tracker_msgs/action/poly_tracker.hpp" namespace kr_mav_manager { @@ -186,6 +187,33 @@ class MAVManagerServices last_cb_ = "estop"; } + void poly_trigger_cb(const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res) + { + (void)req; + if(!have_poly_goal_) + { + RCLCPP_WARN(mav->get_logger(), "No PolyTracker goal received yet; cannot trigger poly_tracker."); + res->success = false; + res->message = "No poly goal available"; + return; + } + + // Send the stored PolyTracker goal via the MAVManager helper + kr_tracker_msgs::action::PolyTracker::Goal goal = latest_poly_goal_; + res->success = mav->sendPolyGoal(goal); + res->message = res->success ? "Poly triggered" : "Poly trigger failed"; + if(res->success) + last_cb_ = "poly_tracker"; + } + + void poly_goal_cb(const kr_tracker_msgs::action::PolyTracker::Goal::SharedPtr goal) + { + latest_poly_goal_ = *goal; + have_poly_goal_ = true; + RCLCPP_DEBUG(mav->get_logger(), "Received PolyTracker goal (stored)"); + } + // Constructor MAVManagerServices(std::shared_ptr m) : mav(m), last_cb_("") { @@ -222,6 +250,15 @@ class MAVManagerServices mav->create_service("~/eland", std::bind(&MAVManagerServices::eland_cb, this, _1, _2)); estop_srv_ = mav->create_service("~/estop", std::bind(&MAVManagerServices::estop_cb, this, _1, _2)); + + // Subscribe to planner published PolyTracker goals (published on topic "tracker_cmd") + poly_goal_sub_ = mav->create_subscription( + "tracker_cmd", 10, std::bind(&MAVManagerServices::poly_goal_cb, this, std::placeholders::_1)); + have_poly_goal_ = false; + + // Provide a trigger service to accept the latest published PolyTracker goal and forward it to the tracker + poly_trigger_srv_ = mav->create_service( + "~/poly_tracker", std::bind(&MAVManagerServices::poly_trigger_cb, this, _1, _2)); } protected: @@ -247,5 +284,9 @@ class MAVManagerServices rclcpp::Service::SharedPtr land_srv_; rclcpp::Service::SharedPtr eland_srv_; rclcpp::Service::SharedPtr estop_srv_; + rclcpp::Service::SharedPtr poly_trigger_srv_; + rclcpp::Subscription::SharedPtr poly_goal_sub_; + kr_tracker_msgs::action::PolyTracker::Goal latest_poly_goal_; + bool have_poly_goal_; }; } // namespace kr_mav_manager diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 046e2234..339fdab1 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -22,6 +22,7 @@ static const std::string null_tracker_str("NullTracker"); static const std::string circle_tracker_str("CircleTracker"); static const std::string lissajous_tracker_str("LissajousTracker"); static const std::string lissajous_adder_str("LissajousAdder"); +static const std::string poly_tracker_str("PolyTracker"); using namespace std::placeholders; @@ -74,6 +75,7 @@ MAVManager::MAVManager() rclcpp_action::create_client(this, "trackers_manager/lissajous_tracker/LissajousTracker"); lissajous_adder_client_ = rclcpp_action::create_client(this, "trackers_manager/lissajous_adder/LissajousAdder"); + poly_tracker_client_ = rclcpp_action::create_client(this, "trackers_manager/poly_tracker/PolyTracker"); float server_wait_timout; server_wait_timout = this->get_parameter("server_wait_timeout").as_double(); @@ -105,6 +107,11 @@ MAVManager::MAVManager() RCLCPP_WARN(this->get_logger(), "LissajousAdder server not found."); } + if(!poly_tracker_client_->wait_for_action_server(server_wait_timeout_duration)) + { + RCLCPP_WARN(this->get_logger(), "PolyTracker server not found."); + } + // Publishers pub_motors_ = this->create_publisher("motors", 10); pub_estop_ = this->create_publisher("estop", 10); @@ -219,6 +226,42 @@ void MAVManager::lissajous_adder_done_callback(const LissajousAdderGoalHandle::W result.result->yaw); } +void MAVManager::poly_tracker_done_callback(const PolyTrackerGoalHandle::WrappedResult &result) +{ + // Log basic info about completion. PolyTracker result contents vary; log duration if present. + try + { + if(result.result) + { + RCLCPP_INFO(this->get_logger(), "PolyTracker completed. (may include duration/summary)"); + } + else + { + RCLCPP_INFO(this->get_logger(), "PolyTracker completed with no result payload."); + } + } + catch(...) { + RCLCPP_INFO(this->get_logger(), "PolyTracker done callback invoked."); + } +} + +bool MAVManager::sendPolyGoal(const PolyTracker::Goal &goal_msg) +{ +// if(!this->motors() || status_ != FLYING) +// { +// RCLCPP_WARN(this->get_logger(), "The robot must be flying before sending a PolyTracker goal."); +// return false; +// } + + auto goal = goal_msg; + auto options = rclcpp_action::Client::SendGoalOptions(); + options.result_callback = std::bind(&MAVManager::poly_tracker_done_callback, this, _1); + + poly_tracker_client_->async_send_goal(goal, options); + + return this->transition(poly_tracker_str); +} + void MAVManager::odometry_cb(nav_msgs::msg::Odometry::ConstSharedPtr msg) { pos_(0) = msg->pose.pose.position.x; diff --git a/trackers/kr_trackers/src/poly_tracker.cpp b/trackers/kr_trackers/src/poly_tracker.cpp index ee507d7e..ed604942 100644 --- a/trackers/kr_trackers/src/poly_tracker.cpp +++ b/trackers/kr_trackers/src/poly_tracker.cpp @@ -216,15 +216,15 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr PolyTracker::update(const nav_ yaw_set_ = false; // ROS_INFO(" yaw_set finished "); time_last_ = time_now; - return std::make_shared(position_cmd_); + return std::make_shared(position_cmd_); } double yaw_temp = cur_yaw_ + (time_now - time_last_).seconds() * init_dyaw_; double desired_yaw = init_final_yaw_ - cur_yaw_ >= 0 ? std::min(yaw_temp, init_final_yaw_) : std::max(yaw_temp, init_final_yaw_); - yaw_yawdot.first = desired_yaw; - yaw_yawdot.second = init_dyaw_; + yaw_yawdot.first = desired_yaw; + yaw_yawdot.second = init_dyaw_; init_yaw_time_ += (time_now - time_last_).seconds(); } @@ -255,7 +255,7 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr PolyTracker::update(const nav_ Eigen::Vector3d dir = t_cur + time_forward_ <= current_trajectory_->traj_dur_ ? current_trajectory_->traj_discrete_.getNextPos(t_cur + time_forward_) - pos : current_trajectory_->traj_discrete_.getNextPos(current_trajectory_->traj_dur_) - pos; - yaw_yawdot = calculate_yaw(dir, (time_now - time_last_).seconds()); + yaw_yawdot = calculate_yaw(dir, (time_now - time_last_).seconds()); break; } From 41e664725dc988f2c51ebce66c06bcc57aed5098 Mon Sep 17 00:00:00 2001 From: Dexter Date: Thu, 23 Oct 2025 10:53:11 -0400 Subject: [PATCH 58/64] Removed debug logging --- .../src/so3cmd_to_betaflight_component.cpp | 2 +- kr_mav_manager/src/manager.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp b/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp index 9423261b..7001f52b 100644 --- a/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp +++ b/interfaces/kr_betaflight_interface/src/so3cmd_to_betaflight_component.cpp @@ -123,7 +123,7 @@ void SO3CmdToBetaflight::odom_callback(const nav_msgs::msg::Odometry::SharedPtr // check if odom is valid if (std::isnan(odom_q_.w()) || std::isnan(odom_q_.x()) || std::isnan(odom_q_.y()) || std::isnan(odom_q_.z())) { - RCLCPP_WARN(this->get_logger(), "Received nan in odom orientation"); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Received nan in odom orientation"); odom_set_ = false; } else diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 339fdab1..7f26066c 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -247,11 +247,11 @@ void MAVManager::poly_tracker_done_callback(const PolyTrackerGoalHandle::Wrapped bool MAVManager::sendPolyGoal(const PolyTracker::Goal &goal_msg) { -// if(!this->motors() || status_ != FLYING) -// { -// RCLCPP_WARN(this->get_logger(), "The robot must be flying before sending a PolyTracker goal."); -// return false; -// } + if(!this->motors() || status_ != FLYING) + { + RCLCPP_WARN(this->get_logger(), "The robot must be flying before sending a PolyTracker goal."); + return false; + } auto goal = goal_msg; auto options = rclcpp_action::Client::SendGoalOptions(); From 6a69458305928946907708e71f924dcf7bf96753 Mon Sep 17 00:00:00 2001 From: Dexter Date: Fri, 24 Oct 2025 11:03:37 -0400 Subject: [PATCH 59/64] Fixed indent --- kr_mav_manager/src/manager.cpp | 3 ++- trackers/kr_trackers/src/poly_tracker.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 7f26066c..7a07dc04 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -75,7 +75,8 @@ MAVManager::MAVManager() rclcpp_action::create_client(this, "trackers_manager/lissajous_tracker/LissajousTracker"); lissajous_adder_client_ = rclcpp_action::create_client(this, "trackers_manager/lissajous_adder/LissajousAdder"); - poly_tracker_client_ = rclcpp_action::create_client(this, "trackers_manager/poly_tracker/PolyTracker"); + poly_tracker_client_ = + rclcpp_action::create_client(this, "trackers_manager/poly_tracker/PolyTracker"); float server_wait_timout; server_wait_timout = this->get_parameter("server_wait_timeout").as_double(); diff --git a/trackers/kr_trackers/src/poly_tracker.cpp b/trackers/kr_trackers/src/poly_tracker.cpp index ed604942..a98c4462 100644 --- a/trackers/kr_trackers/src/poly_tracker.cpp +++ b/trackers/kr_trackers/src/poly_tracker.cpp @@ -223,8 +223,8 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr PolyTracker::update(const nav_ double desired_yaw = init_final_yaw_ - cur_yaw_ >= 0 ? std::min(yaw_temp, init_final_yaw_) : std::max(yaw_temp, init_final_yaw_); - yaw_yawdot.first = desired_yaw; - yaw_yawdot.second = init_dyaw_; + yaw_yawdot.first = desired_yaw; + yaw_yawdot.second = init_dyaw_; init_yaw_time_ += (time_now - time_last_).seconds(); } From b806c58aedac3d9d9dd5018c5f5d6151775e059d Mon Sep 17 00:00:00 2001 From: Dexter Date: Sat, 25 Oct 2025 13:02:10 -0400 Subject: [PATCH 60/64] Fixed polytracker indent --- trackers/kr_trackers/src/poly_tracker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/trackers/kr_trackers/src/poly_tracker.cpp b/trackers/kr_trackers/src/poly_tracker.cpp index a98c4462..b9762025 100644 --- a/trackers/kr_trackers/src/poly_tracker.cpp +++ b/trackers/kr_trackers/src/poly_tracker.cpp @@ -219,7 +219,7 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr PolyTracker::update(const nav_ return std::make_shared(position_cmd_); } - double yaw_temp = cur_yaw_ + (time_now - time_last_).seconds() * init_dyaw_; + double yaw_temp = cur_yaw_ + (time_now - time_last_).seconds() * init_dyaw_; double desired_yaw = init_final_yaw_ - cur_yaw_ >= 0 ? std::min(yaw_temp, init_final_yaw_) : std::max(yaw_temp, init_final_yaw_); From 39e85d6c11ea33bdcc70b1207bc833e66385a0c6 Mon Sep 17 00:00:00 2001 From: Dexter Date: Thu, 30 Oct 2025 17:24:20 -0400 Subject: [PATCH 61/64] Added ros2 velocity tracker --- trackers/kr_trackers/CMakeLists.txt | 1 + trackers/kr_trackers/plugins_description.xml | 7 +- trackers/kr_trackers/src/velocity_tracker.cpp | 127 ++++++++++-------- 3 files changed, 81 insertions(+), 54 deletions(-) diff --git a/trackers/kr_trackers/CMakeLists.txt b/trackers/kr_trackers/CMakeLists.txt index b46f40b8..c3b731b9 100644 --- a/trackers/kr_trackers/CMakeLists.txt +++ b/trackers/kr_trackers/CMakeLists.txt @@ -48,6 +48,7 @@ add_library(${PROJECT_NAME}_plugins SHARED src/line_tracker_min_jerk_server.cpp src/null_tracker.cpp src/poly_tracker.cpp + src/velocity_tracker.cpp src/initial_conditions.cpp) ament_target_dependencies(${PROJECT_NAME}_plugins ${node_dependencies}) diff --git a/trackers/kr_trackers/plugins_description.xml b/trackers/kr_trackers/plugins_description.xml index de0a74d2..b3106477 100644 --- a/trackers/kr_trackers/plugins_description.xml +++ b/trackers/kr_trackers/plugins_description.xml @@ -18,7 +18,12 @@ - Action-based poly tracker implementation + This is a tracker which follows a polynomial trajectory. + + + + This is a tracker which follows a velocity command. diff --git a/trackers/kr_trackers/src/velocity_tracker.cpp b/trackers/kr_trackers/src/velocity_tracker.cpp index 3a2f8471..dc469f20 100644 --- a/trackers/kr_trackers/src/velocity_tracker.cpp +++ b/trackers/kr_trackers/src/velocity_tracker.cpp @@ -1,54 +1,75 @@ -// TODO: convert to ros2 compatible format - -#include -#include -#include -#include -#include +// ROS2 port of VelocityTracker + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_tracker_msgs/msg/velocity_goal.hpp" +#include "kr_tracker_msgs/msg/tracker_status.hpp" +#include "kr_trackers/Tracker.hpp" +#include "tf2/utils.h" +#include class VelocityTracker : public kr_trackers_manager::Tracker { public: VelocityTracker(void); - void Initialize(const ros::NodeHandle &nh); - bool Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); - void Deactivate(void); + void Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent); + bool Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) override; + void Deactivate(void) 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 velocity_cmd_cb(const kr_tracker_msgs::VelocityGoal::ConstPtr &msg); + void velocity_cmd_cb(const kr_tracker_msgs::msg::VelocityGoal::SharedPtr msg); - ros::Subscriber sub_vel_cmd_, sub_position_vel_cmd_; - kr_mav_msgs::PositionCommand position_cmd_; + rclcpp::Subscription::SharedPtr sub_vel_cmd_; + kr_mav_msgs::msg::PositionCommand position_cmd_; bool odom_set_, active_, use_position_gains_; double last_t_; double pos_[3], cur_yaw_; - ros::Time last_cmd_time_; + rclcpp::Time last_cmd_time_; + + double timeout_; - float timeout_; + // ROS2 context + rclcpp::Logger logger_{rclcpp::get_logger("velocity_tracker")}; + rclcpp::Clock::SharedPtr clock_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; }; -VelocityTracker::VelocityTracker(void) : odom_set_(false), active_(false), use_position_gains_(false), last_t_(0) {} +VelocityTracker::VelocityTracker(void) + : odom_set_(false), active_(false), use_position_gains_(false), last_t_(0), timeout_(0.5) +{ +} -void VelocityTracker::Initialize(const ros::NodeHandle &nh) +void VelocityTracker::Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) { - ros::NodeHandle priv_nh(nh, "velocity_tracker"); - priv_nh.param("timeout", timeout_, 0.5f); + auto node = parent.lock(); + if(!node) return; + node_ = node; + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + // declare/read parameter + node_->declare_parameter("timeout", 0.5); + timeout_ = node_->get_parameter("timeout").as_double(); + + // subscribe to the goal (private namespace) + sub_vel_cmd_ = node_->create_subscription( + "~/velocity_tracker/goal", 10, std::bind(&VelocityTracker::velocity_cmd_cb, this, std::placeholders::_1)); - sub_vel_cmd_ = - priv_nh.subscribe("goal", 10, &VelocityTracker::velocity_cmd_cb, this, ros::TransportHints().tcpNoDelay()); + RCLCPP_INFO(logger_, "VelocityTracker initialized (timeout=%f)", timeout_); } -bool VelocityTracker::Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) +bool VelocityTracker::Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) { if(cmd) { position_cmd_.position = cmd->position; position_cmd_.yaw = cmd->yaw; - active_ = true; } else if(odom_set_) @@ -57,10 +78,8 @@ bool VelocityTracker::Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd position_cmd_.position.y = pos_[1]; position_cmd_.position.z = pos_[2]; position_cmd_.yaw = cur_yaw_; - active_ = true; } - return active_; } @@ -71,56 +90,55 @@ void VelocityTracker::Deactivate(void) last_t_ = 0; } -kr_mav_msgs::PositionCommand::ConstPtr VelocityTracker::update(const nav_msgs::Odometry::ConstPtr &msg) +kr_mav_msgs::msg::PositionCommand::ConstSharedPtr VelocityTracker::update(const nav_msgs::msg::Odometry::SharedPtr msg) { pos_[0] = msg->pose.pose.position.x; pos_[1] = msg->pose.pose.position.y; pos_[2] = msg->pose.pose.position.z; - cur_yaw_ = tf::getYaw(msg->pose.pose.orientation); + cur_yaw_ = tf2::getYaw(msg->pose.pose.orientation); odom_set_ = true; if(!active_) - return kr_mav_msgs::PositionCommand::Ptr(); + return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); - if((ros::Time::now() - last_cmd_time_).toSec() > timeout_) + // check timeout against last_cmd_time_ + if((node_->now().seconds() - last_cmd_time_.seconds()) > timeout_) { - // TODO: How much overshoot will this cause at high velocities? - // Ideally ramp down? position_cmd_.velocity.x = 0.0; position_cmd_.velocity.y = 0.0; position_cmd_.velocity.z = 0.0; position_cmd_.yaw_dot = 0.0; - ROS_WARN_THROTTLE(1, "VelocityTracker is active but timed out"); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 1000.0, "VelocityTracker is active but timed out"); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 1000.0, "Time since last command: %f seconds", + (node_->now().seconds() - last_cmd_time_.seconds())); if(use_position_gains_) - position_cmd_.use_msg_gains_flags = kr_mav_msgs::PositionCommand::USE_MSG_GAINS_NONE; + position_cmd_.use_msg_gains_flags = kr_mav_msgs::msg::PositionCommand::USE_MSG_GAINS_NONE; position_cmd_.header.stamp = msg->header.stamp; position_cmd_.header.frame_id = msg->header.frame_id; last_t_ = 0; - return kr_mav_msgs::PositionCommand::ConstPtr(new kr_mav_msgs::PositionCommand(position_cmd_)); + return std::make_shared(position_cmd_); } if(last_t_ == 0) - last_t_ = ros::Time::now().toSec(); + last_t_ = node_->now().seconds(); - const double t_now = ros::Time::now().toSec(); + const double t_now = node_->now().seconds(); const double dt = t_now - last_t_; last_t_ = t_now; if(use_position_gains_) { - position_cmd_.use_msg_gains_flags = kr_mav_msgs::PositionCommand::USE_MSG_GAINS_NONE; - + position_cmd_.use_msg_gains_flags = kr_mav_msgs::msg::PositionCommand::USE_MSG_GAINS_NONE; position_cmd_.position.x = position_cmd_.position.x + dt * position_cmd_.velocity.x; position_cmd_.position.y = position_cmd_.position.y + dt * position_cmd_.velocity.y; position_cmd_.position.z = position_cmd_.position.z + dt * position_cmd_.velocity.z; } else { - position_cmd_.kx[0] = 0, position_cmd_.kx[1] = 0, position_cmd_.kx[2] = 0; - position_cmd_.use_msg_gains_flags = kr_mav_msgs::PositionCommand::USE_MSG_GAINS_POSITION_ALL; - + position_cmd_.kx[0] = 0; position_cmd_.kx[1] = 0; position_cmd_.kx[2] = 0; + position_cmd_.use_msg_gains_flags = kr_mav_msgs::msg::PositionCommand::USE_MSG_GAINS_POSITION_ALL; position_cmd_.position.x = pos_[0]; position_cmd_.position.y = pos_[1]; position_cmd_.position.z = pos_[2]; @@ -129,13 +147,16 @@ kr_mav_msgs::PositionCommand::ConstPtr VelocityTracker::update(const nav_msgs::O position_cmd_.header.stamp = msg->header.stamp; position_cmd_.header.frame_id = msg->header.frame_id; - - return kr_mav_msgs::PositionCommand::ConstPtr(new kr_mav_msgs::PositionCommand(position_cmd_)); + RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000.0, "VelocityTracker dt: %f", dt); + RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000.0, "VelocityTracker commanded vel: (%f, %f, %f), yaw_dot=%f", + position_cmd_.velocity.x, position_cmd_.velocity.y, position_cmd_.velocity.z, position_cmd_.yaw_dot); + RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000.0, "VelocityTracker position command: pos=(%f, %f, %f), yaw=%f", + position_cmd_.position.x, position_cmd_.position.y, position_cmd_.position.z, position_cmd_.yaw); + return std::make_shared(position_cmd_); } -void VelocityTracker::velocity_cmd_cb(const kr_tracker_msgs::VelocityGoal::ConstPtr &msg) +void VelocityTracker::velocity_cmd_cb(const kr_tracker_msgs::msg::VelocityGoal::SharedPtr msg) { - // ROS_INFO("VelocityTracker goal (%2.2f, %2.2f, %2.2f, %2.2f)", msg->vx, msg->vy, msg->vz, msg->vyaw); position_cmd_.velocity.x = msg->vx; position_cmd_.velocity.y = msg->vy; position_cmd_.velocity.z = msg->vz; @@ -143,14 +164,14 @@ void VelocityTracker::velocity_cmd_cb(const kr_tracker_msgs::VelocityGoal::Const use_position_gains_ = msg->use_position_gains; - last_cmd_time_ = ros::Time::now(); + last_cmd_time_ = node_->now(); } -uint8_t VelocityTracker::status() const +uint8_t VelocityTracker::status() { - return active_ ? static_cast(kr_tracker_msgs::TrackerStatus::ACTIVE) : - static_cast(kr_tracker_msgs::TrackerStatus::SUCCEEDED); + return active_ ? static_cast(kr_tracker_msgs::msg::TrackerStatus::ACTIVE) : + static_cast(kr_tracker_msgs::msg::TrackerStatus::SUCCEEDED); } -#include -PLUGINLIB_EXPORT_CLASS(VelocityTracker, kr_trackers_manager::Tracker) +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(VelocityTracker, kr_trackers_manager::Tracker); From b88867df5910ab69a12b5b1f17bfc778aeec22f6 Mon Sep 17 00:00:00 2001 From: Dexter Date: Wed, 5 Nov 2025 16:00:27 -0500 Subject: [PATCH 62/64] Added yaw error tracking in velocity tracker --- trackers/kr_trackers/src/velocity_tracker.cpp | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/trackers/kr_trackers/src/velocity_tracker.cpp b/trackers/kr_trackers/src/velocity_tracker.cpp index dc469f20..7048c1ab 100644 --- a/trackers/kr_trackers/src/velocity_tracker.cpp +++ b/trackers/kr_trackers/src/velocity_tracker.cpp @@ -128,6 +128,8 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr VelocityTracker::update(const const double dt = t_now - last_t_; last_t_ = t_now; + use_position_gains_ = true; + if(use_position_gains_) { position_cmd_.use_msg_gains_flags = kr_mav_msgs::msg::PositionCommand::USE_MSG_GAINS_NONE; @@ -147,12 +149,27 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr VelocityTracker::update(const position_cmd_.header.stamp = msg->header.stamp; position_cmd_.header.frame_id = msg->header.frame_id; + + auto position_cmd = position_cmd_; + + if (position_cmd.yaw_dot == 0.0f) + { + const float target_yaw = position_cmd_.yaw; + const float yaw_error = std::atan2(std::sin(target_yaw - cur_yaw_), std::cos(target_yaw - cur_yaw_)); + const float max_yaw_rate = 0.3f; + const float kp_yaw = 1.0f; // TODO make config + + float yaw_rate = kp_yaw * yaw_error; + yaw_rate = std::max(-max_yaw_rate, std::min(max_yaw_rate, yaw_rate)); // clamp + position_cmd.yaw_dot = yaw_rate; + } + RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000.0, "VelocityTracker dt: %f", dt); RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000.0, "VelocityTracker commanded vel: (%f, %f, %f), yaw_dot=%f", - position_cmd_.velocity.x, position_cmd_.velocity.y, position_cmd_.velocity.z, position_cmd_.yaw_dot); + position_cmd.velocity.x, position_cmd.velocity.y, position_cmd.velocity.z, position_cmd.yaw_dot); RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000.0, "VelocityTracker position command: pos=(%f, %f, %f), yaw=%f", - position_cmd_.position.x, position_cmd_.position.y, position_cmd_.position.z, position_cmd_.yaw); - return std::make_shared(position_cmd_); + position_cmd.position.x, position_cmd.position.y, position_cmd.position.z, position_cmd.yaw); + return std::make_shared(position_cmd); } void VelocityTracker::velocity_cmd_cb(const kr_tracker_msgs::msg::VelocityGoal::SharedPtr msg) From 7503f28f5b5c139e109b707d3214b88f2f48eefd Mon Sep 17 00:00:00 2001 From: Dexter Date: Thu, 5 Mar 2026 19:24:28 +0000 Subject: [PATCH 63/64] Updated new nf motors --- .../config/neurofly.yaml | 26 +- .../config/neurofly_1804.yaml | 35 + .../crsf/crsf_bridge.h | 2 - .../sbus/sbus_bridge.h | 2 - .../scripts/compute_rpm_throttle.py | 20 + .../src/crsf/crsf_bridge.cpp | 5 - .../src/sbus/sbus_bridge.cpp | 3 +- .../kr_matlab_interface/NewOdomEventData.m | 16 - .../kr_matlab_interface/QuadControlRos.m | 142 ---- .../kr_matlab_interface/example_interface.m | 90 --- .../kr_mav_manager.patch.package.xml | 34 - .../kr_matlab_interface/kr_mav_msgs.patch.xml | 36 - interfaces/kr_mavros_interface/CMakeLists.txt | 57 -- interfaces/kr_mavros_interface/README.md | 11 - .../kr_mavros_interface/launch/test.launch | 29 - .../kr_mavros_interface/nodelet_plugin.xml | 7 - interfaces/kr_mavros_interface/package.xml | 26 - .../src/so3cmd_to_mavros_nodelet.cpp | 210 ------ interfaces/kr_python_interface/CMakeLists.txt | 11 - interfaces/kr_python_interface/package.xml | 19 - interfaces/kr_python_interface/setup.py | 11 - .../src/kr_python_interface/__init__.py | 0 .../src/kr_python_interface/mav_example.py | 117 --- .../src/kr_python_interface/mav_interface.py | 142 ---- .../kr_qualcomm_interface/CMakeLists.txt | 66 -- .../cmake/FindSnav.cmake | 21 - .../kr_qualcomm_interface/config/gains.yaml | 15 - .../config/mav_manager_params.yaml | 6 - .../config/tracker_params.yaml | 25 - .../config/trackers.yaml | 11 - .../launch/snav_interface.launch | 88 --- .../launch/snav_publisher.launch | 13 - .../kr_qualcomm_interface/nodelet_plugin.xml | 17 - interfaces/kr_qualcomm_interface/package.xml | 20 - .../src/poscmd_to_snav_nodelet.cpp | 219 ------ .../src/snav_publisher.cpp | 242 ------- .../src/so3cmd_to_qualcomm_nodelet.cpp | 223 ------ .../src/trpycmd_to_snav_nodelet.cpp | 221 ------ .../src/vio_odom_publisher.cpp | 62 -- .../kr_rosflight_interface/CMakeLists.txt | 56 -- interfaces/kr_rosflight_interface/README.md | 7 - .../kr_rosflight_interface/launch/test.launch | 29 - .../kr_rosflight_interface/nodelet_plugin.xml | 7 - interfaces/kr_rosflight_interface/package.xml | 24 - .../src/so3cmd_to_rosflight_nodelet.cpp | 166 ----- interfaces/kr_serial_interface/CMakeLists.txt | 48 -- interfaces/kr_serial_interface/LICENSE | 674 ------------------ .../kr_serial_interface/ASIOSerialDevice.h | 85 --- .../include/kr_serial_interface/comm_types.h | 66 -- .../include/kr_serial_interface/decode_msgs.h | 17 - .../include/kr_serial_interface/encode_msgs.h | 18 - .../kr_serial_interface/serial_interface.h | 13 - .../kr_serial_interface/launch/test.launch | 23 - .../kr_serial_interface/nodelet_plugin.xml | 19 - interfaces/kr_serial_interface/package.xml | 26 - .../src/ASIOSerialDevice.cc | 238 ------- .../src/asctec_serial_interface.cpp | 140 ---- .../kr_serial_interface/src/decode_msgs.cpp | 91 --- .../kr_serial_interface/src/encode_msgs.cpp | 105 --- .../src/quad_decode_msg_nodelet.cpp | 63 -- .../src/quad_encode_msg.cpp | 69 -- .../src/quad_encode_msg_nodelet.cpp | 80 --- .../src/quad_serial_comm_nodelet.cpp | 70 -- kr_quadrotor_simulator/CMakeLists.txt | 53 -- .../config/crazyflie_params.yaml | 13 - .../config/hummingbird_params.yaml | 12 - .../config/qualcomm_quad_params.yaml | 12 - .../kr_quadrotor_simulator/Quadrotor.h | 104 --- kr_quadrotor_simulator/package.xml | 20 - .../src/dynamics/Quadrotor.cpp | 370 ---------- .../src/quadrotor_simulator_base.hpp | 291 -------- .../src/quadrotor_simulator_so3.cpp | 167 ----- .../src/quadrotor_simulator_trpy.cpp | 164 ----- 73 files changed, 70 insertions(+), 5570 deletions(-) create mode 100644 interfaces/kr_betaflight_interface/config/neurofly_1804.yaml create mode 100644 interfaces/kr_betaflight_interface/scripts/compute_rpm_throttle.py delete mode 100644 interfaces/kr_matlab_interface/NewOdomEventData.m delete mode 100644 interfaces/kr_matlab_interface/QuadControlRos.m delete mode 100644 interfaces/kr_matlab_interface/example_interface.m delete mode 100644 interfaces/kr_matlab_interface/kr_mav_manager.patch.package.xml delete mode 100644 interfaces/kr_matlab_interface/kr_mav_msgs.patch.xml delete mode 100644 interfaces/kr_mavros_interface/CMakeLists.txt delete mode 100644 interfaces/kr_mavros_interface/README.md delete mode 100644 interfaces/kr_mavros_interface/launch/test.launch delete mode 100644 interfaces/kr_mavros_interface/nodelet_plugin.xml delete mode 100644 interfaces/kr_mavros_interface/package.xml delete mode 100644 interfaces/kr_mavros_interface/src/so3cmd_to_mavros_nodelet.cpp delete mode 100644 interfaces/kr_python_interface/CMakeLists.txt delete mode 100644 interfaces/kr_python_interface/package.xml delete mode 100644 interfaces/kr_python_interface/setup.py delete mode 100644 interfaces/kr_python_interface/src/kr_python_interface/__init__.py delete mode 100755 interfaces/kr_python_interface/src/kr_python_interface/mav_example.py delete mode 100644 interfaces/kr_python_interface/src/kr_python_interface/mav_interface.py delete mode 100644 interfaces/kr_qualcomm_interface/CMakeLists.txt delete mode 100644 interfaces/kr_qualcomm_interface/cmake/FindSnav.cmake delete mode 100644 interfaces/kr_qualcomm_interface/config/gains.yaml delete mode 100644 interfaces/kr_qualcomm_interface/config/mav_manager_params.yaml delete mode 100644 interfaces/kr_qualcomm_interface/config/tracker_params.yaml delete mode 100644 interfaces/kr_qualcomm_interface/config/trackers.yaml delete mode 100644 interfaces/kr_qualcomm_interface/launch/snav_interface.launch delete mode 100644 interfaces/kr_qualcomm_interface/launch/snav_publisher.launch delete mode 100644 interfaces/kr_qualcomm_interface/nodelet_plugin.xml delete mode 100644 interfaces/kr_qualcomm_interface/package.xml delete mode 100644 interfaces/kr_qualcomm_interface/src/poscmd_to_snav_nodelet.cpp delete mode 100644 interfaces/kr_qualcomm_interface/src/snav_publisher.cpp delete mode 100644 interfaces/kr_qualcomm_interface/src/so3cmd_to_qualcomm_nodelet.cpp delete mode 100644 interfaces/kr_qualcomm_interface/src/trpycmd_to_snav_nodelet.cpp delete mode 100644 interfaces/kr_qualcomm_interface/src/vio_odom_publisher.cpp delete mode 100644 interfaces/kr_rosflight_interface/CMakeLists.txt delete mode 100644 interfaces/kr_rosflight_interface/README.md delete mode 100644 interfaces/kr_rosflight_interface/launch/test.launch delete mode 100644 interfaces/kr_rosflight_interface/nodelet_plugin.xml delete mode 100644 interfaces/kr_rosflight_interface/package.xml delete mode 100644 interfaces/kr_rosflight_interface/src/so3cmd_to_rosflight_nodelet.cpp delete mode 100644 interfaces/kr_serial_interface/CMakeLists.txt delete mode 100644 interfaces/kr_serial_interface/LICENSE delete mode 100644 interfaces/kr_serial_interface/include/kr_serial_interface/ASIOSerialDevice.h delete mode 100644 interfaces/kr_serial_interface/include/kr_serial_interface/comm_types.h delete mode 100644 interfaces/kr_serial_interface/include/kr_serial_interface/decode_msgs.h delete mode 100644 interfaces/kr_serial_interface/include/kr_serial_interface/encode_msgs.h delete mode 100644 interfaces/kr_serial_interface/include/kr_serial_interface/serial_interface.h delete mode 100644 interfaces/kr_serial_interface/launch/test.launch delete mode 100644 interfaces/kr_serial_interface/nodelet_plugin.xml delete mode 100644 interfaces/kr_serial_interface/package.xml delete mode 100644 interfaces/kr_serial_interface/src/ASIOSerialDevice.cc delete mode 100644 interfaces/kr_serial_interface/src/asctec_serial_interface.cpp delete mode 100644 interfaces/kr_serial_interface/src/decode_msgs.cpp delete mode 100644 interfaces/kr_serial_interface/src/encode_msgs.cpp delete mode 100644 interfaces/kr_serial_interface/src/quad_decode_msg_nodelet.cpp delete mode 100644 interfaces/kr_serial_interface/src/quad_encode_msg.cpp delete mode 100644 interfaces/kr_serial_interface/src/quad_encode_msg_nodelet.cpp delete mode 100644 interfaces/kr_serial_interface/src/quad_serial_comm_nodelet.cpp delete mode 100644 kr_quadrotor_simulator/CMakeLists.txt delete mode 100644 kr_quadrotor_simulator/config/crazyflie_params.yaml delete mode 100644 kr_quadrotor_simulator/config/hummingbird_params.yaml delete mode 100644 kr_quadrotor_simulator/config/qualcomm_quad_params.yaml delete mode 100644 kr_quadrotor_simulator/include/kr_quadrotor_simulator/Quadrotor.h delete mode 100644 kr_quadrotor_simulator/package.xml delete mode 100644 kr_quadrotor_simulator/src/dynamics/Quadrotor.cpp delete mode 100644 kr_quadrotor_simulator/src/quadrotor_simulator_base.hpp delete mode 100644 kr_quadrotor_simulator/src/quadrotor_simulator_so3.cpp delete mode 100644 kr_quadrotor_simulator/src/quadrotor_simulator_trpy.cpp diff --git a/interfaces/kr_betaflight_interface/config/neurofly.yaml b/interfaces/kr_betaflight_interface/config/neurofly.yaml index a601ff1f..6e884262 100644 --- a/interfaces/kr_betaflight_interface/config/neurofly.yaml +++ b/interfaces/kr_betaflight_interface/config/neurofly.yaml @@ -3,20 +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 -# 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 + +## 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 + # 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 # when flying in angle mode diff --git a/interfaces/kr_betaflight_interface/config/neurofly_1804.yaml b/interfaces/kr_betaflight_interface/config/neurofly_1804.yaml new file mode 100644 index 00000000..f0649f15 --- /dev/null +++ b/interfaces/kr_betaflight_interface/config/neurofly_1804.yaml @@ -0,0 +1,35 @@ +port_name: /dev/ttyTHS1 +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 + +## 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 +# when flying in angle mode +max_roll_rate: 1000.0 # [deg/s] +max_pitch_rate: 1000.0 # [deg/s] +max_yaw_rate: 1000.0 # [deg/s] +max_roll_angle: 50.0 # [deg] +max_pitch_angle: 50.0 # [deg] +alpha_vbat_filter: 0.01 +perform_thrust_voltage_compensation: false +thrust_ratio_voltage_map_a: -0.17044342 # [1/V] +thrust_ratio_voltage_map_b: 3.10495276 # [-] +n_lipo_cells: 3 # [-] diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_bridge.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_bridge.h index c2b6d18b..a9293e2c 100644 --- a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_bridge.h +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/crsf/crsf_bridge.h @@ -112,8 +112,6 @@ class CrsfBridge : public ProtocolBridgeBase double control_command_timeout_; double rc_timeout_; - double mass_; - bool disable_thrust_mapping_; double max_roll_rate_; diff --git a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_bridge.h b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_bridge.h index 411a14f9..5de2d7ce 100644 --- a/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_bridge.h +++ b/interfaces/kr_betaflight_interface/include/kr_betaflight_interface/sbus/sbus_bridge.h @@ -112,8 +112,6 @@ class SBusBridge : public ProtocolBridgeBase double control_command_timeout_; double rc_timeout_; - double mass_; - bool disable_thrust_mapping_; double max_roll_rate_; diff --git a/interfaces/kr_betaflight_interface/scripts/compute_rpm_throttle.py b/interfaces/kr_betaflight_interface/scripts/compute_rpm_throttle.py new file mode 100644 index 00000000..dcdca7b3 --- /dev/null +++ b/interfaces/kr_betaflight_interface/scripts/compute_rpm_throttle.py @@ -0,0 +1,20 @@ +import pandas as pd +import numpy as np + +df = pd.read_csv("FPVCycle_22.6mm(1920Kv)_Gemfan_Hurricane_51433(5.1x4.33x3)_PropGuard_4S.csv") +esc = df['ESC signal (µs)'].values +rpm = df['Motor Optical Speed (RPM)'].values + +# Filter stable samples +mask = (rpm > 500) & (esc > 1050) +df_f = pd.DataFrame({'esc': esc[mask], 'rpm': rpm[mask]}) + +# Median RPM per throttle step +grouped = df_f.groupby('esc')['rpm'].median().reset_index() + +# Fit +lin_coef = np.polyfit(grouped['esc'], grouped['rpm'], 1) # [a, b] +quad_coef = np.polyfit(grouped['esc'], grouped['rpm'], 2) # [a, b, c] + +print("Linear fit coefficients (a, b):", lin_coef) +print("Quadratic fit coefficients (a, b, c):", quad_coef) \ No newline at end of file diff --git a/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp b/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp index cb30c7ab..e19b6923 100644 --- a/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp +++ b/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp @@ -484,7 +484,6 @@ bool CrsfBridge::loadParameters() node_->declare_parameter("port_name", std::string("/dev/ttyUSB0")); node_->declare_parameter("control_command_timeout", 0.5); node_->declare_parameter("rc_timeout", 0.5); - node_->declare_parameter("mass", 1.0); node_->declare_parameter("disable_thrust_mapping", false); node_->declare_parameter("max_roll_rate", 400.0); node_->declare_parameter("max_pitch_rate", 400.0); @@ -519,10 +518,6 @@ bool CrsfBridge::loadParameters() RCLCPP_ERROR(logger_, "Failed to load parameter: rc_timeout"); all_params_loaded = false; } - if (!node_->get_parameter("mass", mass_)) { - RCLCPP_ERROR(logger_, "Failed to load parameter: mass"); - all_params_loaded = false; - } if (!node_->get_parameter("disable_thrust_mapping", disable_thrust_mapping_)) { RCLCPP_ERROR(logger_, "Failed to load parameter: disable_thrust_mapping"); all_params_loaded = false; diff --git a/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp b/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp index 01fb2f87..e30e48f1 100644 --- a/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp +++ b/interfaces/kr_betaflight_interface/src/sbus/sbus_bridge.cpp @@ -591,7 +591,6 @@ bool SBusBridge::loadParameters() node_->declare_parameter("port_name", std::string("/dev/ttyUSB0")); node_->declare_parameter("control_command_timeout", 0.5); node_->declare_parameter("rc_timeout", 0.5); - node_->declare_parameter("mass", 1.0); node_->declare_parameter("disable_thrust_mapping", false); node_->declare_parameter("max_roll_rate", 400.0); node_->declare_parameter("max_pitch_rate", 400.0); @@ -614,7 +613,7 @@ bool SBusBridge::loadParameters() // Get parameters if(!node_->get_parameter("port_name", port_name_) || !node_->get_parameter("control_command_timeout", control_command_timeout_) || - !node_->get_parameter("rc_timeout", rc_timeout_) || !node_->get_parameter("mass", mass_) || + !node_->get_parameter("rc_timeout", rc_timeout_) || !node_->get_parameter("disable_thrust_mapping", disable_thrust_mapping_) || !node_->get_parameter("max_roll_rate", max_roll_rate_) || !node_->get_parameter("max_pitch_rate", max_pitch_rate_) || !node_->get_parameter("max_yaw_rate", max_yaw_rate_) || diff --git a/interfaces/kr_matlab_interface/NewOdomEventData.m b/interfaces/kr_matlab_interface/NewOdomEventData.m deleted file mode 100644 index dfe4b539..00000000 --- a/interfaces/kr_matlab_interface/NewOdomEventData.m +++ /dev/null @@ -1,16 +0,0 @@ -classdef (ConstructOnLoad) NewOdomEventData < event.EventData - properties - agent_number - position - orientation - end - - methods - function data = NewOdomEventData(agent_number, position, orientation) - data.agent_number = agent_number; - data.position = position; - data.orientation = orientation; - end - end -end - diff --git a/interfaces/kr_matlab_interface/QuadControlRos.m b/interfaces/kr_matlab_interface/QuadControlRos.m deleted file mode 100644 index dd1065ab..00000000 --- a/interfaces/kr_matlab_interface/QuadControlRos.m +++ /dev/null @@ -1,142 +0,0 @@ -classdef QuadControlRos < handle - properties (SetAccess = private) - n_agents = [] - agent = [] - agent_ids = [] - agent_namespace = [] - end - - events - NewOdom - end - - methods - function [obj] = QuadControlRos(hostname, n_agents, agent_namespace) - if nargin < 2 - error('Need hostname and n_agents as argument') - return; - end - - if n_agents < 1 - obj.n_agents = 1; - else - obj.n_agents = n_agents; - end - - if(~isstr(hostname)) - disp('hostname has to be a string, setting to localhost') - hostname = 'localhost'; - end - - if(~isstr(agent_namespace)) - disp('agent_namespace has to be string, using dragonfly') - obj.agent_namespace = 'dragonfly'; - else - obj.agent_namespace = agent_namespace; - end - - obj.agent_ids = 1:n_agents; %TODO add ability to pass non sequential agent ids - rosinit(hostname) - - pause(0.5) - disp('rosinit successful, setting up subscribers, publishers and service clients'); - obj.setup_subs(); - disp('ready'); - end - - function setup_subs(obj) - - for n_ag = 1:obj.n_agents - goto_topic = sprintf('/%s%d/mav_services/goTo',obj.agent_namespace, obj.agent_ids(n_ag)); - obj.agent(n_ag).goto_srv = rossvcclient(goto_topic, 'Timeout', 10); - - %TODO use kr_multi_mav_manager interface? Will reduce number of - %subscribers in MATLAB - motors_topic = sprintf('/%s%d/mav_services/motors',obj.agent_namespace, obj.agent_ids(n_ag)); - obj.agent(n_ag).motors_srv = rossvcclient(motors_topic, 'Timeout', 10); - - takeoff_topic = sprintf('/%s%d/mav_services/takeoff',obj.agent_namespace, obj.agent_ids(n_ag)); - obj.agent(n_ag).takeoff_srv = rossvcclient(takeoff_topic, 'Timeout', 10); - - twist_topic = sprintf('/%s%d/cmd_vel',obj.agent_namespace, obj.agent_ids(n_ag)); - obj.agent(n_ag).twist_pub = rospublisher(twist_topic,'geometry_msgs/Twist'); - end - - %Setup callbacks at the end - for n_ag = 1:obj.n_agents - odom_topic = sprintf('/%s%d/odom',obj.agent_namespace, obj.agent_ids(n_ag)); - obj.agent(n_ag).odom_sub = rossubscriber(odom_topic, {@obj.odom_sub_cb, n_ag}); - end - - disp('Starting sub/pub'); - pause(1) - end - - function delete(obj) - % obj is always scalar - for n_ag = 1:obj.n_agents - delete(obj.agent(n_ag).odom_sub); - end - rosshutdown; - end - - function[] = send_twist(obj,agent_number, twist) - twistdata = rosmessage('geometry_msgs/Twist'); - twistdata.Linear.X = twist(1); - twistdata.Linear.Y = twist(2); - twistdata.Linear.Z = twist(3); - - twistdata.Angular.Z = twist(4); - - send(obj.agent(agent_number).twist_pub,twistdata); - end - - function[] = send_zero_twist(obj, agent_number) - twistdata = rosmessage('geometry_msgs/Twist'); - twistdata.Linear.X = 0; - twistdata.Linear.Y = 0; - twistdata.Linear.Z = 0; - - twistdata.Angular.Z = 0; - - send(obj.agent(agent_number).twist_pub,twistdata); - end - - function[response] = send_wp(obj, agent_number, wp) - request = rosmessage(obj.agent(agent_number).goto_srv); - request.Goal = [wp(1), wp(2), wp(3), wp(4)]; - response = call(obj.agent(agent_number).goto_srv, request); - end - - function[response] = motors(obj, agent_number, motors) - request = rosmessage(obj.agent(agent_number).motors_srv); - request.Data = motors; - response = call(obj.agent(agent_number).motors_srv, request); - end - - function[response] = takeoff(obj, agent_number) - request = rosmessage(obj.agent(agent_number).takeoff_srv); - response = call(obj.agent(agent_number).takeoff_srv, request); - end - - function [odom] = getOdom(obj,agent_number) - odom = obj.agent(agent_number).odom; - end - - function[] = odom_sub_cb(obj, ~, msg, agent_number) - obj.agent(agent_number).odom = msg; - - pt = msg.Pose.Pose.Position; - qt = msg.Pose.Pose.Orientation; - position = [pt.X, pt.Y, pt.Z]; - orientation = [qt.W, qt.X, qt.Y, qt.Z]; - - %Broadcast a new odometry event with corresponding data - odom_event_data = NewOdomEventData(agent_number, position, orientation); - notify(obj, 'NewOdom', odom_event_data); - - end - - end -end - diff --git a/interfaces/kr_matlab_interface/example_interface.m b/interfaces/kr_matlab_interface/example_interface.m deleted file mode 100644 index 78a4e9c1..00000000 --- a/interfaces/kr_matlab_interface/example_interface.m +++ /dev/null @@ -1,90 +0,0 @@ -function example_interface(hostname, n_agents) - -% pass hostname as 'localhost' for simulator running on same maching -if nargin < 2 - error('Need hostname and n_agents as argument') - return; -end - -if(~isnumeric(n_agents)) - disp('n_agents has to be an integer') - return; -end - -if n_agents < 1 - n_agents = 1; -end - -if(~ischar(hostname)) - disp('hostname has to be a string, setting to localhost') - hostname = 'localhost'; -end - -%Prepare plotting -fig1 = figure; -ax1 = axes('XLim',[-10, 10], 'YLim', [-10, 10], 'ZLim', [-10,10], 'Parent', fig1); - -quad_obj = QuadControlRos(hostname,n_agents, 'dragonfly'); - -%Create mesh visualization handles for each agent -for n_ag = 1:n_agents - vis_handles.ax_handles(n_ag) = plotTransforms([rand(1), rand(1), rand(1)], [1,0,0,0], 'MeshFilePath','multirotor.stl', 'MeshColor', [rand(1) rand(1) rand(1)], 'FrameSize', 2, 'Parent', ax1); -end - -%Add event listner to update Mesh pose on new odometry -new_odom_listner_handle = addlistener(quad_obj,'NewOdom',@(quad_obj,evnt)odomEventcallbackMethod(quad_obj,evnt,vis_handles)); - -%Turn on motors and takeoff -for n_ag = 1:n_agents - resp = quad_obj.motors(n_ag, 1); - pause(0.1) %Might need longer pause on real platforms for motors to idle - resp = quad_obj.takeoff(n_ag); -end - -for i=1:30 - - agent_num = randi([1, n_agents]); - curr_odom = quad_obj.getOdom(agent_num); - if ~isempty(curr_odom) - curr_position = curr_odom.Pose.Pose.Position; - txt = sprintf('%d pose %g %g %g %g',agent_num, curr_position.X, curr_position.Y, curr_position.Z); - disp(txt); - end - quad_obj.send_twist(agent_num, [rand(1),rand(1),0,0]) - - pause(0.4) -end - -%Hover all robots with zero vel -for n=1:n_agents - quad_obj.send_zero_twist(n); -end - -% send to wp -for n=1:n_agents - response = quad_obj.send_wp(n, [randi([0,2]), randi([0,2]),0,1.0]); -end - -clear quad_obj -end - -function odomEventcallbackMethod(src,evnt, vis_handles) -agent_number = evnt.agent_number; -position = evnt.position; -orientation = evnt.orientation; - -if ~isempty(vis_handles) - %Take a look at - %'R2019a/toolbox/robotics/robotcore/+robotics/+core/+internal/+visualization/TransformPainter.m' - % move method in above file - - ax = vis_handles.ax_handles(agent_number); - hBodyToInertial = findobj(ax, 'Type', 'hgtransform','Tag', robotics.core.internal.visualization.TransformPainter.GraphicsObjectTags.BodyToInertial); - - if ~isempty(hBodyToInertial) - tform = quat2tform(orientation); - tform(1:3,4) = position; - set(hBodyToInertial(agent_number), 'Matrix', tform); - end -end -end \ No newline at end of file diff --git a/interfaces/kr_matlab_interface/kr_mav_manager.patch.package.xml b/interfaces/kr_matlab_interface/kr_mav_manager.patch.package.xml deleted file mode 100644 index 91aee522..00000000 --- a/interfaces/kr_matlab_interface/kr_mav_manager.patch.package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - kr_mav_manager - 1.0.0 - - This package provides tools to interface with the kr_mav_control stack - - Justin Thomas - BSD - - - - catkin - - roscpp - geometry_msgs - nav_msgs - std_msgs - sensor_msgs - kr_mav_msgs - kr_tracker_msgs - message_generation - std_srvs - - - roscpp - geometry_msgs - nav_msgs - std_msgs - sensor_msgs - kr_mav_msgs - kr_tracker_msgs - message_runtime - std_srvs - diff --git a/interfaces/kr_matlab_interface/kr_mav_msgs.patch.xml b/interfaces/kr_matlab_interface/kr_mav_msgs.patch.xml deleted file mode 100644 index 16962ae9..00000000 --- a/interfaces/kr_matlab_interface/kr_mav_msgs.patch.xml +++ /dev/null @@ -1,36 +0,0 @@ - - kr_mav_msgs - 1.0.0 - kr_mav_msgs - Kartik Mohta - Michael Watterson - Justin Thomas - - BSD - - http://ros.org/wiki/kr_mav_msgs - - - Kartik Mohta - - - catkin - - - message_generation - geometry_msgs - nav_msgs - std_msgs - - - - message_runtime - geometry_msgs - nav_msgs - std_msgs - - - - - - diff --git a/interfaces/kr_mavros_interface/CMakeLists.txt b/interfaces/kr_mavros_interface/CMakeLists.txt deleted file mode 100644 index d81ec578..00000000 --- a/interfaces/kr_mavros_interface/CMakeLists.txt +++ /dev/null @@ -1,57 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(kr_mavros_interface) - -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) -endif() - -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) - -find_package( - catkin REQUIRED - COMPONENTS roscpp - nav_msgs - geometry_msgs - kr_mav_msgs - nodelet) -find_package(Eigen3 REQUIRED) - -# If mavros_msgs not found, don't build but do not cause a build error -find_package(mavros_msgs) - -if(NOT ${mavros_msgs_FOUND}) - message(WARNING "NOTE: mavros_msgs not found so not building kr_mavros_interface") -else() - include_directories(${catkin_INCLUDE_DIRS} ${mavros_msgs_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) - - catkin_package( - INCLUDE_DIRS - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - roscpp - nav_msgs - geometry_msgs - kr_mav_msgs - mavros_msgs - nodelet - DEPENDS - EIGEN3) - - add_library(${PROJECT_NAME} src/so3cmd_to_mavros_nodelet.cpp) - add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - - install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - - install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -endif() diff --git a/interfaces/kr_mavros_interface/README.md b/interfaces/kr_mavros_interface/README.md deleted file mode 100644 index 2c91d049..00000000 --- a/interfaces/kr_mavros_interface/README.md +++ /dev/null @@ -1,11 +0,0 @@ -# kr_mavros_interface - -This node translates `kr_mav_msgs/SO3Command` to `mavros_msgs/AttitudeTarget`. - -Please note that your flight controller yaw orientation estimation may be -different to your odom system orientation estimation. We should transform our -`kr_mav_msgs/SO3Command` into the flight controller yaw before publishing it. - -#### `mavros_msgs` requirement - -This node requires `mavros_msgs` to be present when building. If `mavros_msgs` is not found when building the first time, a warning is given and nothing in this package is built. If `mavros_msgs` is installed after this, force a recheck by adding `--force-cmake` to the `catkin_make`/`catkin build` command. diff --git a/interfaces/kr_mavros_interface/launch/test.launch b/interfaces/kr_mavros_interface/launch/test.launch deleted file mode 100644 index f334abdb..00000000 --- a/interfaces/kr_mavros_interface/launch/test.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/interfaces/kr_mavros_interface/nodelet_plugin.xml b/interfaces/kr_mavros_interface/nodelet_plugin.xml deleted file mode 100644 index 498cfd33..00000000 --- a/interfaces/kr_mavros_interface/nodelet_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - This reformats an so3_command and publishes it on compatabile mavros topics - - - diff --git a/interfaces/kr_mavros_interface/package.xml b/interfaces/kr_mavros_interface/package.xml deleted file mode 100644 index 618f6205..00000000 --- a/interfaces/kr_mavros_interface/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - kr_mavros_interface - 1.0.0 - kr_mavros_interface - Anurag Makineni - Justin Thomas - Kartik Mohta - - BSD - - - catkin - - - roscpp - nav_msgs - geometry_msgs - kr_mav_msgs - mavros_msgs - nodelet - - - - - - diff --git a/interfaces/kr_mavros_interface/src/so3cmd_to_mavros_nodelet.cpp b/interfaces/kr_mavros_interface/src/so3cmd_to_mavros_nodelet.cpp deleted file mode 100644 index 1c951469..00000000 --- a/interfaces/kr_mavros_interface/src/so3cmd_to_mavros_nodelet.cpp +++ /dev/null @@ -1,210 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -class SO3CmdToMavros : public nodelet::Nodelet -{ - public: - void onInit(void); - - private: - void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg); - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - void imu_callback(const sensor_msgs::Imu::ConstPtr &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_; - - ros::Publisher attitude_raw_pub_; - ros::Publisher odom_pose_pub_; // For sending PoseStamped to firmware - - ros::Subscriber so3_cmd_sub_; - ros::Subscriber odom_sub_; - ros::Subscriber imu_sub_; - - double so3_cmd_timeout_; - ros::Time last_so3_cmd_time_; - kr_mav_msgs::SO3Command last_so3_cmd_; -}; - -void SO3CmdToMavros::odom_callback(const nav_msgs::Odometry::ConstPtr &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 = boost::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::Imu::ConstPtr &pose) -{ - imu_q_ = Eigen::Quaterniond(pose->orientation.w, pose->orientation.x, pose->orientation.y, pose->orientation.z); - imu_set_ = true; - - if(so3_cmd_set_ && ((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_)) - { - ROS_INFO("so3_cmd timeout. %f seconds since last command", (ros::Time::now() - last_so3_cmd_time_).toSec()); - const auto last_so3_cmd_ptr = boost::make_shared(last_so3_cmd_); - - so3_cmd_callback(last_so3_cmd_ptr); - } -} - -void SO3CmdToMavros::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg) -{ - // both imu_q_ and odom_q_ would be uninitialized if not set - if(!imu_set_) - { - ROS_WARN("Did not receive any imu messages from %s", imu_sub_.getTopic().c_str()); - return; - } - - if(!odom_set_) - { - ROS_WARN("Did not receive any odom messages from %s", odom_sub_.getTopic().c_str()); - 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 tf::Quaternion - tf::Quaternion imu_tf = tf::Quaternion(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w()); - tf::Quaternion odom_tf = tf::Quaternion(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; - tf::Matrix3x3(imu_tf).getRPY(imu_roll, imu_pitch, imu_yaw); - tf::Matrix3x3(odom_tf).getRPY(odom_roll, odom_pitch, odom_yaw); - - // create only yaw tf:Quaternions - tf::Quaternion imu_tf_yaw; - tf::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 tf::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 - { - ROS_WARN_THROTTLE(1, "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 (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 = boost::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_ = ros::Time::now(); - so3_cmd_set_ = true; -} - -void SO3CmdToMavros::onInit(void) -{ - ros::NodeHandle priv_nh(getPrivateNodeHandle()); - - // get thrust scaling parameters - if(priv_nh.getParam("num_props", num_props_)) - ROS_INFO("Got number of props: %d", num_props_); - else - ROS_ERROR("Must set num_props param"); - - if(priv_nh.getParam("kf", kf_)) - ROS_INFO("Using kf=%g so that prop speed = sqrt(f / num_props / kf) to scale force to speed.", kf_); - else - ROS_ERROR("Must set kf param for thrust scaling. Motor speed = sqrt(thrust / num_props / kf)"); - - ROS_ASSERT_MSG(kf_ > 0, "kf must be positive. kf = %g", kf_); - - // get thrust scaling parameters - if(priv_nh.getParam("lin_cof_a", lin_cof_a_) && priv_nh.getParam("lin_int_b", lin_int_b_)) - ROS_INFO("Using %g*x + %g to scale prop speed to att_throttle.", lin_cof_a_, lin_int_b_); - else - ROS_ERROR("Must set coefficients for thrust scaling (scaling from rotor " - "velocity (rad/s) to att_throttle for pixhawk)"); - - // get param for so3 command timeout duration - priv_nh.param("so3_cmd_timeout", so3_cmd_timeout_, 0.25); - - odom_set_ = false; - imu_set_ = false; - so3_cmd_set_ = false; - - attitude_raw_pub_ = priv_nh.advertise("attitude_raw", 10); - - odom_pose_pub_ = priv_nh.advertise("odom_pose", 10); - - so3_cmd_sub_ = - priv_nh.subscribe("so3_cmd", 10, &SO3CmdToMavros::so3_cmd_callback, this, ros::TransportHints().tcpNoDelay()); - - odom_sub_ = priv_nh.subscribe("odom", 10, &SO3CmdToMavros::odom_callback, this, ros::TransportHints().tcpNoDelay()); - - imu_sub_ = priv_nh.subscribe("imu", 10, &SO3CmdToMavros::imu_callback, this, ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(SO3CmdToMavros, nodelet::Nodelet); diff --git a/interfaces/kr_python_interface/CMakeLists.txt b/interfaces/kr_python_interface/CMakeLists.txt deleted file mode 100644 index 55bee454..00000000 --- a/interfaces/kr_python_interface/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(kr_python_interface) - -find_package(catkin REQUIRED) -catkin_python_setup() - -catkin_package() - -install(PROGRAMS src/kr_python_interface/mav_interface.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/interfaces/kr_python_interface/package.xml b/interfaces/kr_python_interface/package.xml deleted file mode 100644 index 96a5d077..00000000 --- a/interfaces/kr_python_interface/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - kr_python_interface - 0.1.0 - Provides a python interface for kr_mav_control - Dinesh Thakur - - BSD - - Dinesh Thakur - Laura Jarin-Lipschitz - - catkin - - rospy - geometry_msgs - kr_tracker_msgs - kr_mav_manager - - diff --git a/interfaces/kr_python_interface/setup.py b/interfaces/kr_python_interface/setup.py deleted file mode 100644 index ed907f82..00000000 --- a/interfaces/kr_python_interface/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python3 - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['kr_python_interface'], - package_dir={'': 'src'}, -) - -setup(**d) diff --git a/interfaces/kr_python_interface/src/kr_python_interface/__init__.py b/interfaces/kr_python_interface/src/kr_python_interface/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/interfaces/kr_python_interface/src/kr_python_interface/mav_example.py b/interfaces/kr_python_interface/src/kr_python_interface/mav_example.py deleted file mode 100755 index c609f49e..00000000 --- a/interfaces/kr_python_interface/src/kr_python_interface/mav_example.py +++ /dev/null @@ -1,117 +0,0 @@ -#! /usr/bin/env python3 -import rospy -import numpy as np -import random - -from geometry_msgs.msg import Twist, Pose -from nav_msgs.msg import Odometry -from std_srvs.srv import SetBool, Trigger -from kr_tracker_msgs.msg import TrajectoryTrackerAction, TrajectoryTrackerGoal, CircleTrackerAction, CircleTrackerGoal - -from kr_python_interface.mav_interface import KrMavInterface - -def main(): - rospy.init_node('mav_example', anonymous=True) - - # Creating MAV objects - mav_namespace = 'dragonfly' - mav_id = 1 - mav_obj = KrMavInterface('dragonfly', mav_id) - - # Motor On / Take Off - mav_obj.motors_on() - mav_obj.take_off() - - rospy.sleep(1) - - #Send waypoint (open loop, have to sleep until finished) - mav_obj.send_wp(4.0, 0.0, 1.0, 0.0) - rospy.sleep(4) - - #Send waypoint blocking - mav_obj.send_wp_block(0.0, 0.0, 1.0, 0.0, 0.5, 0.3, False) #x, y, z, yaw, vel, acc, relative - - #Send random twist commands - for i in range(20): - #get current odometry - curr_odom = mav_obj.get_odom(); - curr_position = curr_odom.pose.pose.position; - rospy.loginfo('pose %g %g %g', curr_position.x, curr_position.y, curr_position.z); - - mav_obj.set_vel(random.uniform(0.1,1.0), random.uniform(0.1,1.0), 0, 0, 0, 0) #vx, vy, vz, - - rospy.sleep(0.3) - - #Send waypoint blocking - mav_obj.send_wp_block(0.0, 0.0, 1.0, 0.0, 1.0, 0.5, False) #x, y, z, yaw, vel, acc, relative - - #Run circle tracker - mav_obj.hover() - goal = CircleTrackerGoal() - goal.Ax = -1.0 - goal.Ay = -1.0 - goal.T = 4.0 - num_repetitions = 1 - goal.duration = goal.T*num_repetitions - mav_obj.circle_tracker_client.cancel_all_goals() - rospy.sleep(0.1) - mav_obj.circle_tracker_client.send_goal(goal) - rospy.logwarn("Send circle") - - success = mav_obj.transition_service_call('CircleTracker') - if not success: - rospy.logwarn("Failed to transition to circle tracker (is there an active goal?)") - - rospy.logwarn("Waiting for circle to run") - mav_obj.circle_tracker_client.wait_for_result() - - #Send waypoint blocking - mav_obj.send_wp_block(0.0, 0.0, 1.5, 0.0, 1.0, 0.5, False) #x, y, z, yaw, vel, acc, relative - - #Send multiple waypoints by fitting traj - goal = TrajectoryTrackerGoal() - wp = Pose() - wp.position.x = 0.0 - wp.position.y = 0.0 - wp.position.z = 2.0 - goal.waypoints.append(wp) - - wp1 = Pose() - wp1.position.x = 0.0 - wp1.position.y = 1.0 - wp1.position.z = 1.5 - goal.waypoints.append(wp1) - - wp2 = Pose() - wp2.position.x = 2.0 - wp2.position.y = 2.0 - wp2.position.z = 1.0 - goal.waypoints.append(wp2) - - wp3 = Pose() - wp3.position.x = 3.0 - wp3.position.y = 0.0 - wp3.position.z = 0.5 - goal.waypoints.append(wp3) - - mav_obj.traj_tracker_client.send_goal(goal) - success = mav_obj.transition_service_call('TrajectoryTracker') - if not success: - rospy.logwarn("Failed to transition to trajectory tracker (is there an active goal?)") - - rospy.logwarn("Waiting for traj to run") - mav_obj.traj_tracker_client.wait_for_result() - - #Send waypoint blocking - mav_obj.send_wp_block(0.0, 0.0, 1.0, 0.0, 1.5, 0.5, False) #x, y, z, yaw, vel, acc, relative - - # Land / Motors off - mav_obj.land() - rospy.sleep(3) - mav_obj.motors_off() - -if __name__ == '__main__': - try : - main() - except rospy.ROSInterruptException : - pass diff --git a/interfaces/kr_python_interface/src/kr_python_interface/mav_interface.py b/interfaces/kr_python_interface/src/kr_python_interface/mav_interface.py deleted file mode 100644 index 6856a235..00000000 --- a/interfaces/kr_python_interface/src/kr_python_interface/mav_interface.py +++ /dev/null @@ -1,142 +0,0 @@ -#! /usr/bin/env python3 -from __future__ import print_function - -import rospy -import actionlib - -from actionlib_msgs.msg import GoalStatus -from geometry_msgs.msg import Twist -from nav_msgs.msg import Odometry, Path -from kr_tracker_msgs.msg import TrajectoryTrackerAction, TrajectoryTrackerGoal, CircleTrackerAction, CircleTrackerGoal, LineTrackerAction, LineTrackerGoal - -from kr_tracker_msgs.srv import Transition -from std_srvs.srv import Trigger, SetBool -from kr_mav_manager.srv import Vec4 - -class KrMavInterface(object): - - def __init__(self, mav_namespace='dragonfly', id=0): - self.mav_namespace = mav_namespace - self.id = id - - self.mav_name = self.mav_namespace + str(self.id) - - self.pub_vel = rospy.Publisher('/' + self.mav_name + '/cmd_vel', Twist, queue_size=10) - - self.odom = Odometry() - self.sub_odom = rospy.Subscriber('/' + self.mav_name + '/odom', Odometry, self.update_odom) - - self.line_tracker_client = actionlib.SimpleActionClient(self.mav_name + - '/trackers_manager/line_tracker_min_jerk/LineTracker', LineTrackerAction) - self.traj_tracker_client = actionlib.SimpleActionClient(self.mav_name + - '/trackers_manager/trajectory_tracker/TrajectoryTracker', TrajectoryTrackerAction) - self.circle_tracker_client = actionlib.SimpleActionClient(self.mav_name + - '/trackers_manager/circle_tracker/CircleTracker', CircleTrackerAction) - self.traj_tracker_status = "" - - def update_odom(self, msg): - self.odom = msg - - def get_odom(self): - return self.odom - - def motors_on(self): - try: - motors = rospy.ServiceProxy('/' + self.mav_name + '/mav_services/motors', SetBool) - resp = motors(True) - rospy.loginfo(resp) - except rospy.ServiceException as e: - rospy.logwarn("Service call failed: %s"%e) - return 'aborted' - - def motors_off(self): - try: - motors = rospy.ServiceProxy('/' + self.mav_name + '/mav_services/motors', SetBool) - resp = motors(False) - rospy.loginfo(resp) - except rospy.ServiceException as e: - rospy.logwarn("Service call failed: %s"%e) - return 'aborted' - - def take_off(self): - try: - takeoff = rospy.ServiceProxy('/' + self.mav_name + '/mav_services/takeoff', Trigger) - resp = takeoff() - rospy.loginfo(resp) - except rospy.ServiceException as e: - rospy.logwarn("Service call failed: %s"%e) - return 'aborted' - - def hover(self): - rospy.logwarn("Transition to hover") - self.traj_tracker_client.cancel_all_goals() - rospy.wait_for_service('/' + self.mav_name + '/mav_services/hover') - try: - srv = rospy.ServiceProxy('/' + self.mav_name + '/mav_services/hover', Trigger) - resp = srv() - rospy.loginfo(resp) - except rospy.ServiceException as e: - rospy.logwarn("Service call failed: %s" % e) - return False - - def land(self): - rospy.logwarn("Transition to land") - self.traj_tracker_client.cancel_all_goals() - rospy.wait_for_service('/' + self.mav_name + '/mav_services/land') - try: - srv = rospy.ServiceProxy('/' + self.mav_name + '/mav_services/land', Trigger) - resp = srv() - rospy.loginfo(resp) - except rospy.ServiceException as e: - rospy.logwarn("Service call failed: %s" % e) - return False - - def set_vel(self, vx=0.0, vy=0.0, vz=0.0, ax=0.0, ay=0.0, az=0.0): - command = Twist() - command.linear.x = vx - command.linear.y = vy - command.linear.z = vz - command.angular.x = ax - command.angular.y = ay - command.angular.z = az - - self.pub_vel.publish(command) - - def send_wp(self, x, y, z, yaw): - try: - rospy.wait_for_service('/' + self.mav_name + '/mav_services/goTo') - srv = rospy.ServiceProxy('/' + self.mav_name +'/mav_services/goTo', Vec4) - resp = srv([x, y, z, yaw]) - rospy.loginfo(resp) - except rospy.ServiceException as e: - rospy.logwarn("Service call failed: %s" % e) - return False - - def send_wp_block(self, x, y, z, yaw, vel=1.0, acc=0.5, relative=False): - goal = LineTrackerGoal(); - goal.x = x; - goal.y = y; - goal.z = z; - goal.yaw = yaw; - goal.v_des = 1.0; - goal.a_des = 0.5; - goal.relative = False; - - self.line_tracker_client.send_goal(goal) - self.transition_service_call('LineTrackerMinJerk') - rospy.logwarn("Waiting for Line Tracker to complete") - self.line_tracker_client.wait_for_result() - - def transition_service_call(self, tracker_name): - rospy.loginfo('waiting for transition service for ' + tracker_name) - rospy.wait_for_service('/' + self.mav_name +'/trackers_manager/transition') - try: - tt = rospy.ServiceProxy('/' + self.mav_name +'/trackers_manager/transition', Transition) - resp = tt('kr_trackers/' + tracker_name) - rospy.loginfo(resp) - if resp.success == False or "already active" in resp.message: - return False - return True - except rospy.ServiceException as e: - rospy.logwarn("Service call failed: %s" % e) - return False diff --git a/interfaces/kr_qualcomm_interface/CMakeLists.txt b/interfaces/kr_qualcomm_interface/CMakeLists.txt deleted file mode 100644 index 5bd86bc8..00000000 --- a/interfaces/kr_qualcomm_interface/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(kr_qualcomm_interface) - -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) -endif() - -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) - -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake/") - -find_package( - catkin REQUIRED - COMPONENTS roscpp - kr_mav_msgs - nav_msgs - nodelet - tf_conversions) -find_package(Eigen3 REQUIRED) - -find_package(Snav) -if(NOT ${Snav_FOUND}) - message(WARNING "NOTE: snav not found so not building kr_qualcomm_interface") -else() - include_directories(${Snav_INCLUDE_DIR}) - include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) - - catkin_package( - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - roscpp - kr_mav_msgs - nav_msgs - nodelet - tf_conversions - DEPENDS - EIGEN3) - - add_library(${PROJECT_NAME} src/so3cmd_to_qualcomm_nodelet.cpp src/trpycmd_to_snav_nodelet.cpp - src/poscmd_to_snav_nodelet.cpp) - add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Snav_LIBRARY}) - - add_executable(vio_odom_publisher src/vio_odom_publisher.cpp) - target_link_libraries(vio_odom_publisher ${catkin_LIBRARIES} ${Snav_LIBRARY}) - - add_executable(snav_publisher src/snav_publisher.cpp) - add_dependencies(snav_publisher ${catkin_EXPORTED_TARGETS}) - target_link_libraries(snav_publisher ${catkin_LIBRARIES} ${Snav_LIBRARY}) - - install( - TARGETS ${PROJECT_NAME} vio_odom_publisher snav_publisher - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - - install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) - install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -endif() diff --git a/interfaces/kr_qualcomm_interface/cmake/FindSnav.cmake b/interfaces/kr_qualcomm_interface/cmake/FindSnav.cmake deleted file mode 100644 index 4df6b9cf..00000000 --- a/interfaces/kr_qualcomm_interface/cmake/FindSnav.cmake +++ /dev/null @@ -1,21 +0,0 @@ -FIND_PATH(Snav_INCLUDE_DIR - NAMES - snav/snapdragon_navigator.h - PATHS - /usr/include - NO_DEFAULT_PATH -) - -FIND_LIBRARY(Snav_LIBRARY - NAMES - snav_arm - PATHS - /usr/lib - NO_DEFAULT_PATH -) - -IF(Snav_INCLUDE_DIR AND Snav_LIBRARY) - SET(Snav_FOUND TRUE) -ELSE(Snav_INCLUDE_DIR AND Snav_LIBRARY) - SET(Snav_FOUND FALSE) -ENDIF(Snav_INCLUDE_DIR AND Snav_LIBRARY) diff --git a/interfaces/kr_qualcomm_interface/config/gains.yaml b/interfaces/kr_qualcomm_interface/config/gains.yaml deleted file mode 100644 index f8868f19..00000000 --- a/interfaces/kr_qualcomm_interface/config/gains.yaml +++ /dev/null @@ -1,15 +0,0 @@ -gains: - pos: {x: 3.4, y: 3.4, z: 5.4} - vel: {x: 2.8, y: 2.8, z: 3.0} - ki: {x: 0.00, y: 0.00, z: 0.00, yaw: 0.01} - kib: {x: 0.00, y: 0.00, z: 0.00} - rot: {x: 1.5, y: 1.5, z: 1.0} - ang: {x: 0.13, y: 0.13, z: 0.1} - -corrections: - kf: 0.0e-08 - r: 0.0 - p: 0.0 - -max_pos_int: 0.5 -max_pos_int_b: 0.5 \ No newline at end of file diff --git a/interfaces/kr_qualcomm_interface/config/mav_manager_params.yaml b/interfaces/kr_qualcomm_interface/config/mav_manager_params.yaml deleted file mode 100644 index 40685c46..00000000 --- a/interfaces/kr_qualcomm_interface/config/mav_manager_params.yaml +++ /dev/null @@ -1,6 +0,0 @@ -server_wait_timeout: 0.5 -need_imu: false -need_output_data: true -use_attitude_safety_catch: true -max_attitude_angle: 0.43 -takeoff_height: 0.2 diff --git a/interfaces/kr_qualcomm_interface/config/tracker_params.yaml b/interfaces/kr_qualcomm_interface/config/tracker_params.yaml deleted file mode 100644 index 64c0d4b9..00000000 --- a/interfaces/kr_qualcomm_interface/config/tracker_params.yaml +++ /dev/null @@ -1,25 +0,0 @@ -# This should contain tracker parameters - -line_tracker_distance: - default_v_des: 1.0 - default_a_des: 0.5 - epsilon: 0.1 - -line_tracker_min_jerk: - default_v_des: 1.0 - default_a_des: 0.5 - default_yaw_v_des: 0.8 - default_yaw_a_des: 0.2 - -trajectory_tracker: - max_vel_des: 0.5 - max_acc_des: 0.3 - -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_qualcomm_interface/config/trackers.yaml b/interfaces/kr_qualcomm_interface/config/trackers.yaml deleted file mode 100644 index 9fc1551d..00000000 --- a/interfaces/kr_qualcomm_interface/config/trackers.yaml +++ /dev/null @@ -1,11 +0,0 @@ -trackers: - - kr_trackers/LineTrackerMinJerk - - kr_trackers/LineTrackerDistance -# - kr_trackers/LineTrackerTrapezoid -# - kr_trackers/LineTrackerYaw - - kr_trackers/VelocityTrackerAction - - kr_trackers/NullTracker - - kr_trackers/CircleTracker - - kr_trackers/TrajectoryTracker - - kr_trackers/SmoothVelTracker - - kr_trackers/LissajousTracker diff --git a/interfaces/kr_qualcomm_interface/launch/snav_interface.launch b/interfaces/kr_qualcomm_interface/launch/snav_interface.launch deleted file mode 100644 index 22a28a25..00000000 --- a/interfaces/kr_qualcomm_interface/launch/snav_interface.launch +++ /dev/null @@ -1,88 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/interfaces/kr_qualcomm_interface/launch/snav_publisher.launch b/interfaces/kr_qualcomm_interface/launch/snav_publisher.launch deleted file mode 100644 index f8e6fc10..00000000 --- a/interfaces/kr_qualcomm_interface/launch/snav_publisher.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/interfaces/kr_qualcomm_interface/nodelet_plugin.xml b/interfaces/kr_qualcomm_interface/nodelet_plugin.xml deleted file mode 100644 index d82b4fb3..00000000 --- a/interfaces/kr_qualcomm_interface/nodelet_plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - This take a so3_command and publishes it to the robot using Qualcomm's API - - - - - This take a trpy_command and publishes it to the robot using Qualcomm's Snav API - - - - - This take a pos_command and publishes it to the robot using Qualcomm's Snav API - - - diff --git a/interfaces/kr_qualcomm_interface/package.xml b/interfaces/kr_qualcomm_interface/package.xml deleted file mode 100644 index de2a3a80..00000000 --- a/interfaces/kr_qualcomm_interface/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - kr_qualcomm_interface - 0.0.0 - The kr_qualcomm_interface package - - Dinesh Thakur - - BSD - - catkin - nav_msgs - nodelet - kr_mav_msgs - roscpp - tf_conversions - - - - diff --git a/interfaces/kr_qualcomm_interface/src/poscmd_to_snav_nodelet.cpp b/interfaces/kr_qualcomm_interface/src/poscmd_to_snav_nodelet.cpp deleted file mode 100644 index 6877352e..00000000 --- a/interfaces/kr_qualcomm_interface/src/poscmd_to_snav_nodelet.cpp +++ /dev/null @@ -1,219 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -class PosCmdToSnav : public nodelet::Nodelet -{ - public: - void onInit(void); - - private: - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - void imu_callback(const sensor_msgs::Imu::ConstPtr &pose); - void position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); - void enable_motors_callback(const std_msgs::Bool::ConstPtr &msg); - void motors_on(); - void motors_off(); - - // controller state - SnavCachedData *snav_cached_data_struct_; - - bool pos_cmd_set_; - - ros::Subscriber odom_sub_, imu_sub_, position_cmd_sub_, enable_motors_sub_; - - int motor_status_; - - double pos_cmd_timeout_; - ros::Time last_pos_cmd_time_; - kr_mav_msgs::PositionCommand last_pos_cmd_; -}; - -void PosCmdToSnav::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) -{ - // Send min thrust as heartbeat - if(motor_status_ && ((ros::Time::now() - last_pos_cmd_time_).toSec() >= pos_cmd_timeout_)) - { - sn_send_thrust_att_ang_vel_command(0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); - } -} - -void PosCmdToSnav::imu_callback(const sensor_msgs::Imu::ConstPtr &pose) -{ - if(pos_cmd_set_ && ((ros::Time::now() - last_pos_cmd_time_).toSec() >= pos_cmd_timeout_)) - { - ROS_DEBUG("pos_cmd timeout. %f seconds since last command", (ros::Time::now() - last_pos_cmd_time_).toSec()); - const auto last_pos_cmd_ptr = boost::make_shared(last_pos_cmd_); - - position_cmd_callback(last_pos_cmd_ptr); - } -} - -void PosCmdToSnav::motors_on() -{ - // call the update 0 success - int res_update = sn_update_data(); - if(res_update == -1) - { - ROS_ERROR("Likely failure in snav, ensure it is running"); - return; - } - - switch(snav_cached_data_struct_->general_status.props_state) - { - case SN_PROPS_STATE_NOT_SPINNING: - { - // sn_send_thrust_att_ang_vel_command (0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); - int ret = sn_spin_props(); - if(ret == -1) - ROS_ERROR("Not able to send spinning command"); - break; - } - case SN_PROPS_STATE_STARTING: - { - ROS_WARN("Propellers are starting to spin"); - break; - } - case SN_PROPS_STATE_SPINNING: - { - ROS_INFO("Propellers are spinning"); - motor_status_ = 1; - break; - } - default: - ROS_ERROR("SN_PROPS_STATE_UNKNOWN"); - } -} - -void PosCmdToSnav::motors_off() -{ - do - { - // call the update, 0-success - int res_update = sn_update_data(); - if(res_update == -1) - { - ROS_ERROR("Likely failure in snav, ensure it is running"); - } - - // send minimum thrust and identity attitude - sn_send_thrust_att_ang_vel_command(0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); - - // stop the props, 0-success - int r = sn_stop_props(); - if(r == 0) - motor_status_ = 0; - else if(r == -1) - { - ROS_ERROR("Not able to send switch off propellers"); - } - } while(snav_cached_data_struct_->general_status.props_state == SN_PROPS_STATE_SPINNING); - - // check the propellers status - if(snav_cached_data_struct_->general_status.props_state == SN_PROPS_STATE_SPINNING) - { - ROS_ERROR("All the propellers are still spinning"); - motor_status_ = 1; - } - else - ROS_INFO("All the propellers are now off"); -} - -void PosCmdToSnav::enable_motors_callback(const std_msgs::Bool::ConstPtr &msg) -{ - if(msg->data) - { - ROS_INFO("Enabling motors"); - for(int i = 0; i < 50; i++) - { - sn_send_thrust_att_ang_vel_command(0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); - if(!motor_status_) - motors_on(); - else - return; - ros::Duration(0.05).sleep(); - } - } - else - { - ROS_INFO("Disabling motors"); - motors_off(); - } -} - -void PosCmdToSnav::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &pos) -{ - float yaw = static_cast(pos->yaw); - float yaw_rate = static_cast(pos->yaw_dot); - - float x = static_cast(pos->position.x); - float y = static_cast(pos->position.y); - float z = static_cast(pos->position.z); - - float xd = static_cast(pos->velocity.x); - float yd = static_cast(pos->velocity.y); - float zd = static_cast(pos->velocity.z); - - float xdd = static_cast(pos->acceleration.x); - float ydd = static_cast(pos->acceleration.y); - float zdd = static_cast(pos->acceleration.z); - - if(sn_get_flight_data_ptr(sizeof(SnavCachedData), &snav_cached_data_struct_) != 0) - { - ROS_ERROR("failed to get flight data pointer"); - return; - } - - int update_ret = sn_update_data(); - - if(update_ret != 0) - { - ROS_ERROR("detected likely failure in SN, Ensure it is running"); - } - else - { - sn_send_trajectory_tracking_command(SN_POSITION_CONTROL_VIO, SN_TRAJ_DEFAULT, x, y, z, xd, yd, zd, xdd, ydd, zdd, - yaw, yaw_rate); - } - - if(!pos_cmd_set_) - pos_cmd_set_ = true; - - // save last pos_cmd - last_pos_cmd_ = *pos; - last_pos_cmd_time_ = ros::Time::now(); -} - -void PosCmdToSnav::onInit(void) -{ - ros::NodeHandle priv_nh(getPrivateNodeHandle()); - - // get param for pos command timeout duration - priv_nh.param("pos_cmd_timeout", pos_cmd_timeout_, 0.25); - - pos_cmd_set_ = false; - motor_status_ = 0; - snav_cached_data_struct_ = NULL; - - if(sn_get_flight_data_ptr(sizeof(SnavCachedData), &snav_cached_data_struct_) != 0) - { - ROS_ERROR("Failed to get flight data pointer!"); - return; - } - - odom_sub_ = priv_nh.subscribe("odom", 10, &PosCmdToSnav::odom_callback, this, ros::TransportHints().tcpNoDelay()); - - imu_sub_ = priv_nh.subscribe("imu", 10, &PosCmdToSnav::imu_callback, this, ros::TransportHints().tcpNoDelay()); - - position_cmd_sub_ = priv_nh.subscribe("position_cmd", 10, &PosCmdToSnav::position_cmd_callback, this, - ros::TransportHints().tcpNoDelay()); - enable_motors_sub_ = - priv_nh.subscribe("motors", 2, &PosCmdToSnav::enable_motors_callback, this, ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(PosCmdToSnav, nodelet::Nodelet) diff --git a/interfaces/kr_qualcomm_interface/src/snav_publisher.cpp b/interfaces/kr_qualcomm_interface/src/snav_publisher.cpp deleted file mode 100644 index 106a0a3d..00000000 --- a/interfaces/kr_qualcomm_interface/src/snav_publisher.cpp +++ /dev/null @@ -1,242 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -class SnavSampler -{ - public: - SnavSampler(ros::NodeHandle &nh, ros::NodeHandle &pnh); - void rpmTimerCallback(const ros::TimerEvent &event); - void statusTimerCallback(const ros::TimerEvent &event); - void attitudeTimerCallback(const ros::TimerEvent &event); - - private: - bool get_snav_offset(); - - SnavCachedData *sn_struct_; - ros::Duration snav_offset_; - - ros::Publisher motor_speeds_pub_; - ros::Timer rpm_timer_; - - ros::Publisher battery_pub_; - ros::Publisher joy_pub_; - ros::Publisher on_ground_pub_; - ros::Publisher props_state_pub_; - ros::Timer status_timer_; - - ros::Publisher attitude_estimate_pub_; - ros::Timer attitude_estimate_timer_; -}; - -SnavSampler::SnavSampler(ros::NodeHandle &nh, ros::NodeHandle &pnh) : sn_struct_(NULL) -{ - if(sn_get_flight_data_ptr(sizeof(SnavCachedData), &sn_struct_) != 0) - throw "failed to get flight data ptr"; - if(!get_snav_offset()) - throw "unable to obtain snav offset due to update failure"; - - float rpm_rate; - float status_rate; - float attitude_rate; - pnh.param("rpm_rate", rpm_rate, 100.0); - pnh.param("status_rate", status_rate, 5.0); - pnh.param("attitude_rate", attitude_rate, 100.0); - - if(rpm_rate > 1e-3) - { - ROS_INFO("Publish motor_rpm at %4.2fHz", rpm_rate); - motor_speeds_pub_ = nh.advertise("motor_rpm", 2); - rpm_timer_ = nh.createTimer(ros::Duration(1.0 / rpm_rate), &SnavSampler::rpmTimerCallback, this); - } - if(status_rate > 1e-3) - { - ROS_INFO("Publish status (battery, spektrum_joy, on_ground, props_state) at %4.2fHz", status_rate); - battery_pub_ = nh.advertise("battery", 2); - joy_pub_ = nh.advertise("spektrum_joy", 2); - on_ground_pub_ = nh.advertise("on_ground", 2); - props_state_pub_ = nh.advertise("props_state", 2); - status_timer_ = nh.createTimer(ros::Duration(1.0 / status_rate), &SnavSampler::statusTimerCallback, this); - } - if(attitude_rate > 1e-3) - { - ROS_INFO("Publish attitude estimates at %4.2fHz", attitude_rate); - attitude_estimate_pub_ = nh.advertise("attitude_estimate", 2); - attitude_estimate_timer_ = - nh.createTimer(ros::Duration(1.0 / attitude_rate), &SnavSampler::attitudeTimerCallback, this); - } -} - -bool SnavSampler::get_snav_offset() -{ - if(sn_update_data() != 0) - { - ROS_ERROR("snav data retrieval failure"); - return false; - } - struct timespec time_realtime; - clock_gettime(CLOCK_REALTIME, &time_realtime); - ros::Time realtime(time_realtime.tv_sec, time_realtime.tv_nsec); - - int64_t gen_timestamp_ns = sn_struct_->general_status.time * 1000; - ros::Time sntime; - sntime.fromNSec(gen_timestamp_ns); - snav_offset_ = realtime - sntime; - - if(realtime < sntime) - ROS_WARN("snavtime larger than realtime, potential overflow"); - - return true; -} - -void SnavSampler::attitudeTimerCallback(const ros::TimerEvent &event) -{ - if(sn_update_data() != 0) - ROS_ERROR("snav data retrieval failure"); - else - { - int64_t attitude_timestamp_ns = sn_struct_->attitude_estimate.time * 1000; - ros::Time attitude_time; - attitude_time.fromNSec(attitude_timestamp_ns); - attitude_time += snav_offset_; - - geometry_msgs::QuaternionStamped attitude_msg; - attitude_msg.header.stamp = attitude_time; - tf2::Matrix3x3 R(sn_struct_->attitude_estimate.rotation_matrix[0], sn_struct_->attitude_estimate.rotation_matrix[1], - sn_struct_->attitude_estimate.rotation_matrix[2], sn_struct_->attitude_estimate.rotation_matrix[3], - sn_struct_->attitude_estimate.rotation_matrix[4], sn_struct_->attitude_estimate.rotation_matrix[5], - sn_struct_->attitude_estimate.rotation_matrix[6], sn_struct_->attitude_estimate.rotation_matrix[7], - sn_struct_->attitude_estimate.rotation_matrix[8]); - tf2::Quaternion q; - R.getRotation(q); - attitude_msg.quaternion.x = q.getX(); - attitude_msg.quaternion.y = q.getY(); - attitude_msg.quaternion.z = q.getZ(); - attitude_msg.quaternion.w = q.getW(); - attitude_estimate_pub_.publish(attitude_msg); - } -} - -void SnavSampler::rpmTimerCallback(const ros::TimerEvent &event) -{ - if(sn_update_data() != 0) - ROS_ERROR("snav data retrieval failure"); - else - { - int64_t esc_timestamp_ns = sn_struct_->esc_raw.time * 1000; - ros::Time esc_time; - esc_time.fromNSec(esc_timestamp_ns); - esc_time += snav_offset_; - - kr_mav_msgs::MotorRPM speed; - speed.header.stamp = esc_time; - int16_t *rpm = sn_struct_->esc_raw.rpm; - speed.motor_count = 4; - speed.rpm[0] = rpm[0]; - speed.rpm[1] = rpm[1]; - speed.rpm[2] = rpm[2]; - speed.rpm[3] = rpm[3]; - motor_speeds_pub_.publish(speed); - } -} - -void SnavSampler::statusTimerCallback(const ros::TimerEvent &event) -{ - if(sn_update_data() != 0) - ROS_ERROR("snav data retrieval failure"); - else - { - int64_t gen_timestamp_ns = sn_struct_->general_status.time * 1000; - ros::Time data_time; - data_time.fromNSec(gen_timestamp_ns); - data_time += snav_offset_; - - // Battery - sensor_msgs::BatteryState bat_state; - bat_state.header.stamp = data_time; - bat_state.voltage = sn_struct_->general_status.voltage; - bat_state.current = -sn_struct_->general_status.current; - battery_pub_.publish(bat_state); - - // Spektrum joy - int64_t rc_timestamp_ns = sn_struct_->spektrum_rc_0_raw.time * 1000; - ros::Time rc_time; - rc_time.fromNSec(rc_timestamp_ns); - rc_time += snav_offset_; - - int32_t rc_status = sn_struct_->data_status.spektrum_rc_0_status; - uint8_t num_channels = sn_struct_->spektrum_rc_0_raw.num_channels; - - uint8_t max_channels = 16; - if(rc_status == SN_DATA_VALID && num_channels < max_channels) - { - sensor_msgs::Joy joy; - joy.header.frame_id = "spektrum"; - joy.header.stamp = rc_time; - joy.axes.reserve(5); - joy.axes.push_back(sn_struct_->spektrum_rc_0_raw.vals[0]); - joy.axes.push_back(sn_struct_->spektrum_rc_0_raw.vals[3]); - joy.axes.push_back(sn_struct_->spektrum_rc_0_raw.vals[1]); - joy.axes.push_back(sn_struct_->spektrum_rc_0_raw.vals[2]); - joy.axes.push_back(sn_struct_->spektrum_rc_0_raw.vals[7]); - - joy.buttons.reserve(2); - joy.buttons.push_back(sn_struct_->spektrum_rc_0_raw.vals[4]); - joy.buttons.push_back(sn_struct_->spektrum_rc_0_raw.vals[5]); - - joy_pub_.publish(joy); - } - - // On Ground - bool on_ground = sn_struct_->general_status.on_ground; - on_ground_pub_.publish(on_ground); - - // Prop status - std_msgs::String props_state_msg; - if((SnPropsState)sn_struct_->general_status.props_state == SN_PROPS_STATE_NOT_SPINNING) - { - props_state_msg.data = "NOT_SPINNING"; - } - else if((SnPropsState)sn_struct_->general_status.props_state == SN_PROPS_STATE_STARTING) - { - props_state_msg.data = "STARTING"; - } - else if((SnPropsState)sn_struct_->general_status.props_state == SN_PROPS_STATE_SPINNING) - { - props_state_msg.data = "SPINNING"; - } - else - { - props_state_msg.data = "UNKNOWN"; - } - props_state_pub_.publish(props_state_msg); - } -} - -int main(int argc, char *argv[]) -{ - ros::init(argc, argv, "snav_publisher"); - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - - try - { - SnavSampler snav_sampler(nh, pnh); - ros::spin(); - } - catch(const char *e) - { - ROS_ERROR("%s", e); - } - return 0; -} diff --git a/interfaces/kr_qualcomm_interface/src/so3cmd_to_qualcomm_nodelet.cpp b/interfaces/kr_qualcomm_interface/src/so3cmd_to_qualcomm_nodelet.cpp deleted file mode 100644 index af49612f..00000000 --- a/interfaces/kr_qualcomm_interface/src/so3cmd_to_qualcomm_nodelet.cpp +++ /dev/null @@ -1,223 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -class SO3CmdToQualcomm : public nodelet::Nodelet -{ - public: - void onInit(void); - - private: - void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg); - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - void imu_callback(const sensor_msgs::Imu::ConstPtr &pose); - void so3_cmd_to_qc_interface(const kr_mav_msgs::SO3Command::ConstPtr &msg); - void motors_on(); - void motors_off(); - - // controller state - SnavCachedData *snav_cached_data_struct_; - - bool odom_set_, imu_set_, so3_cmd_set_; - Eigen::Quaterniond odom_q_, imu_q_; - - ros::Subscriber so3_cmd_sub_; - ros::Subscriber odom_sub_; - ros::Subscriber imu_sub_; - - int motor_status_; - - double so3_cmd_timeout_; - ros::Time last_so3_cmd_time_; - kr_mav_msgs::SO3Command last_so3_cmd_; -}; - -void SO3CmdToQualcomm::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) -{ - if(!odom_set_) - odom_set_ = true; - - odom_q_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, - odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); - - if(so3_cmd_set_ && ((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_)) - { - ROS_DEBUG("so3_cmd timeout. %f seconds since last command", (ros::Time::now() - last_so3_cmd_time_).toSec()); - const auto last_so3_cmd_ptr = boost::make_shared(last_so3_cmd_); - - so3_cmd_callback(last_so3_cmd_ptr); - } -} - -void SO3CmdToQualcomm::imu_callback(const sensor_msgs::Imu::ConstPtr &pose) -{ - if(!imu_set_) - imu_set_ = true; - - imu_q_ = Eigen::Quaterniond(pose->orientation.w, pose->orientation.x, pose->orientation.y, pose->orientation.z); - - if(so3_cmd_set_ && ((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_)) - { - ROS_DEBUG("so3_cmd timeout. %f seconds since last command", (ros::Time::now() - last_so3_cmd_time_).toSec()); - const auto last_so3_cmd_ptr = boost::make_shared(last_so3_cmd_); - - so3_cmd_callback(last_so3_cmd_ptr); - } -} - -void SO3CmdToQualcomm::motors_on() -{ - // call the update 0 success - int res_update = sn_update_data(); - if(res_update == -1) - { - ROS_ERROR("Likely failure in snav, ensure it is running"); - return; - } - - switch(snav_cached_data_struct_->general_status.props_state) - { - case SN_PROPS_STATE_NOT_SPINNING: - { - sn_send_thrust_att_ang_vel_command(0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); - int ret = sn_spin_props(); - if(ret == -1) - ROS_ERROR("Not able to send spinning command"); - break; - } - case SN_PROPS_STATE_STARTING: - { - ROS_WARN("Propellers are starting to spin"); - break; - } - case SN_PROPS_STATE_SPINNING: - { - ROS_INFO("Propellers are spinning"); - motor_status_ = 1; - break; - } - default: - ROS_ERROR("SN_PROPS_STATE_UNKNOWN"); - } -} - -void SO3CmdToQualcomm::motors_off() -{ - do - { - // call the update, 0-success - int res_update = sn_update_data(); - if(res_update == -1) - { - ROS_ERROR("Likely failure in snav, ensure it is running"); - } - - // send minimum thrust and identity attitude - sn_send_thrust_att_ang_vel_command(0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); - - // stop the props, 0-success - int r = sn_stop_props(); - if(r == 0) - motor_status_ = 0; - else if(r == -1) - { - ROS_ERROR("Not able to send switch off propellers"); - } - } while(snav_cached_data_struct_->general_status.props_state == SN_PROPS_STATE_SPINNING); - - // check the propellers status - if(snav_cached_data_struct_->general_status.props_state == SN_PROPS_STATE_SPINNING) - { - ROS_ERROR("All the propellers are still spinning"); - motor_status_ = 1; - } -} - -void SO3CmdToQualcomm::so3_cmd_to_qc_interface(const kr_mav_msgs::SO3Command::ConstPtr &msg) -{ - // 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); - const Eigen::Vector3d ang_vel(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); - - // convert to tf::Quaternion - tf::Quaternion imu_tf = tf::Quaternion(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w()); - tf::Quaternion odom_tf = tf::Quaternion(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w()); - - const Eigen::Matrix3d R_cur(odom_q_); - - double throttle = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); - - // convert throttle in grams - throttle = throttle * 1000 / 9.81; - - int res_update = sn_update_data(); - if(res_update == -1) - { - ROS_ERROR("Likely failure in snav, ensure it is running"); - return; - } - - int r = sn_send_thrust_att_ang_vel_command(throttle, q_des.w(), q_des.x(), q_des.y(), q_des.z(), ang_vel(0), - ang_vel(1), ang_vel(2)); - if(r == -1) - ROS_ERROR("Control command not send"); -} - -void SO3CmdToQualcomm::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg) -{ - if(!so3_cmd_set_) - so3_cmd_set_ = true; - - // switch on motors - if(msg->aux.enable_motors && !motor_status_) - motors_on(); - else if(!msg->aux.enable_motors) - motors_off(); - - so3_cmd_to_qc_interface(msg); - - // save last so3_cmd - last_so3_cmd_ = *msg; - last_so3_cmd_time_ = ros::Time::now(); -} - -void SO3CmdToQualcomm::onInit(void) -{ - ros::NodeHandle priv_nh(getPrivateNodeHandle()); - - // get param for so3 command timeout duration - priv_nh.param("so3_cmd_timeout", so3_cmd_timeout_, 0.25); - - odom_set_ = false; - imu_set_ = false; - so3_cmd_set_ = false; - motor_status_ = 0; - snav_cached_data_struct_ = NULL; - - if(sn_get_flight_data_ptr(sizeof(SnavCachedData), &snav_cached_data_struct_) != 0) - { - ROS_ERROR("Failed to get flight data pointer!"); - return; - } - // attitude_raw_pub_ = - // priv_nh.advertise("attitude_raw", 10); - - so3_cmd_sub_ = - priv_nh.subscribe("so3_cmd", 10, &SO3CmdToQualcomm::so3_cmd_callback, this, ros::TransportHints().tcpNoDelay()); - - odom_sub_ = priv_nh.subscribe("odom", 10, &SO3CmdToQualcomm::odom_callback, this, ros::TransportHints().tcpNoDelay()); - - imu_sub_ = priv_nh.subscribe("imu", 10, &SO3CmdToQualcomm::imu_callback, this, ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(SO3CmdToQualcomm, nodelet::Nodelet); diff --git a/interfaces/kr_qualcomm_interface/src/trpycmd_to_snav_nodelet.cpp b/interfaces/kr_qualcomm_interface/src/trpycmd_to_snav_nodelet.cpp deleted file mode 100644 index e26525a1..00000000 --- a/interfaces/kr_qualcomm_interface/src/trpycmd_to_snav_nodelet.cpp +++ /dev/null @@ -1,221 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -class TRPYCmdToSnav : public nodelet::Nodelet -{ - public: - void onInit(void); - - private: - void trpy_cmd_callback(const kr_mav_msgs::TRPYCommand::ConstPtr &msg); - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - void imu_callback(const sensor_msgs::Imu::ConstPtr &pose); - void trpy_cmd_to_qc_interface(const kr_mav_msgs::TRPYCommand::ConstPtr &msg); - void motors_on(); - void motors_off(); - - // controller state - SnavCachedData *snav_cached_data_struct_; - - bool odom_set_, imu_set_, trpy_cmd_set_; - Eigen::Quaterniond odom_q_, imu_q_; - - ros::Subscriber trpy_cmd_sub_; - ros::Subscriber odom_sub_; - ros::Subscriber imu_sub_; - - int motor_status_; - - double trpy_cmd_timeout_; - ros::Time last_trpy_cmd_time_; - kr_mav_msgs::TRPYCommand last_trpy_cmd_; -}; - -void TRPYCmdToSnav::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) -{ - if(!odom_set_) - odom_set_ = true; - - odom_q_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, - odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); - if(trpy_cmd_set_ && ((ros::Time::now() - last_trpy_cmd_time_).toSec() >= trpy_cmd_timeout_)) - { - ROS_DEBUG("trpy_cmd timeout. %f seconds since last command", (ros::Time::now() - last_trpy_cmd_time_).toSec()); - const auto last_trpy_cmd_ptr = boost::make_shared(last_trpy_cmd_); - - trpy_cmd_callback(last_trpy_cmd_ptr); - } -} - -void TRPYCmdToSnav::imu_callback(const sensor_msgs::Imu::ConstPtr &pose) -{ - if(!imu_set_) - imu_set_ = true; - - imu_q_ = Eigen::Quaterniond(pose->orientation.w, pose->orientation.x, pose->orientation.y, pose->orientation.z); - - if(trpy_cmd_set_ && ((ros::Time::now() - last_trpy_cmd_time_).toSec() >= trpy_cmd_timeout_)) - { - ROS_DEBUG("trpy_cmd timeout. %f seconds since last command", (ros::Time::now() - last_trpy_cmd_time_).toSec()); - const auto last_trpy_cmd_ptr = boost::make_shared(last_trpy_cmd_); - - trpy_cmd_callback(last_trpy_cmd_ptr); - } -} - -void TRPYCmdToSnav::motors_on() -{ - // call the update 0 success - int res_update = sn_update_data(); - if(res_update == -1) - { - ROS_ERROR("Likely failure in snav, ensure it is running"); - return; - } - - switch(snav_cached_data_struct_->general_status.props_state) - { - case SN_PROPS_STATE_NOT_SPINNING: - { - sn_send_thrust_att_ang_vel_command(0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); - int ret = sn_spin_props(); - if(ret == -1) - ROS_ERROR("Not able to send spinning command"); - break; - } - case SN_PROPS_STATE_STARTING: - { - ROS_WARN("Propellers are starting to spin"); - break; - } - case SN_PROPS_STATE_SPINNING: - { - ROS_INFO("Propellers are spinning"); - motor_status_ = 1; - break; - } - default: - ROS_ERROR("SN_PROPS_STATE_UNKNOWN"); - } -} - -void TRPYCmdToSnav::motors_off() -{ - do - { - // call the update 0 success - int res_update = sn_update_data(); - if(res_update == -1) - { - ROS_ERROR("Likely failure in snav, ensure it is running"); - return; - } - - // send minimum thrust and identity attitude - sn_send_thrust_att_ang_vel_command(0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); - - // stop the props 0 success - int r = sn_stop_props(); - if(r == 0) - motor_status_ = 0; - else if(r == -1) - { - ROS_ERROR("Not able to send switch off propellers"); - } - } while(snav_cached_data_struct_->general_status.props_state == SN_PROPS_STATE_SPINNING); - - // check the propellers status - if(snav_cached_data_struct_->general_status.props_state == SN_PROPS_STATE_SPINNING) - { - ROS_ERROR("All the propellers are still spinning"); - motor_status_ = 1; - } - else - ROS_INFO("All the propellers are now off"); -} - -void TRPYCmdToSnav::trpy_cmd_to_qc_interface(const kr_mav_msgs::TRPYCommand::ConstPtr &msg) -{ - const tf::Quaternion q_rot = tf::createQuaternionFromRPY(msg->roll, msg->pitch, msg->yaw); - Eigen::Quaterniond q_des; - tf::quaternionTFToEigen(q_rot, q_des); - - const Eigen::Vector3d ang_vel(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); - - // convert to tf::Quaternion - tf::Quaternion imu_tf = tf::Quaternion(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w()); - tf::Quaternion odom_tf = tf::Quaternion(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w()); - - double throttle = msg->thrust; - - // convert throttle in grams - throttle = throttle * 1000 / 9.81; - - int res_update = sn_update_data(); - if(res_update == -1) - { - ROS_ERROR("Likely failure in snav, ensure it is running"); - return; - } - - int r = sn_send_thrust_att_ang_vel_command(throttle, q_des.w(), q_des.x(), q_des.y(), q_des.z(), ang_vel(0), - ang_vel(1), ang_vel(2)); - if(r == -1) - ROS_ERROR("Control command not send"); -} - -void TRPYCmdToSnav::trpy_cmd_callback(const kr_mav_msgs::TRPYCommand::ConstPtr &msg) -{ - if(!trpy_cmd_set_) - trpy_cmd_set_ = true; - - // switch on motors - if(msg->aux.enable_motors && !motor_status_) - motors_on(); - else if(!msg->aux.enable_motors) - motors_off(); - - trpy_cmd_to_qc_interface(msg); - - // save last trpy_cmd - last_trpy_cmd_ = *msg; - last_trpy_cmd_time_ = ros::Time::now(); -} - -void TRPYCmdToSnav::onInit(void) -{ - ros::NodeHandle priv_nh(getPrivateNodeHandle()); - - // get param for trpy command timeout duration - priv_nh.param("trpy_cmd_timeout", trpy_cmd_timeout_, 0.25); - - odom_set_ = false; - imu_set_ = false; - trpy_cmd_set_ = false; - motor_status_ = 0; - snav_cached_data_struct_ = NULL; - if(sn_get_flight_data_ptr(sizeof(SnavCachedData), &snav_cached_data_struct_) != 0) - { - ROS_ERROR("\nFailed to get flight data pointer!\n"); - return; - } - - trpy_cmd_sub_ = - priv_nh.subscribe("trpy_cmd", 10, &TRPYCmdToSnav::trpy_cmd_callback, this, ros::TransportHints().tcpNoDelay()); - - odom_sub_ = priv_nh.subscribe("odom", 10, &TRPYCmdToSnav::odom_callback, this, ros::TransportHints().tcpNoDelay()); - - imu_sub_ = priv_nh.subscribe("imu", 10, &TRPYCmdToSnav::imu_callback, this, ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(TRPYCmdToSnav, nodelet::Nodelet); diff --git a/interfaces/kr_qualcomm_interface/src/vio_odom_publisher.cpp b/interfaces/kr_qualcomm_interface/src/vio_odom_publisher.cpp deleted file mode 100644 index 888e86a5..00000000 --- a/interfaces/kr_qualcomm_interface/src/vio_odom_publisher.cpp +++ /dev/null @@ -1,62 +0,0 @@ -#include -#include -#include -#include - -int main(int argc, char *argv[]) -{ - ros::init(argc, argv, "vio_odom_publisher"); - ros::NodeHandle nh; - ros::Publisher odom_pub = nh.advertise("odom", 10); - ros::Rate loop_rate(100); - - int update_ret; - float roll, pitch, yaw; - while(ros::ok()) - { - // read the flight data - SnavCachedData *snav_data = NULL; - if(sn_get_flight_data_ptr(sizeof(SnavCachedData), &snav_data) != 0) - { - ROS_ERROR("failed to get flight data ptr"); - continue; - } - update_ret = sn_update_data(); - if(update_ret != 0) - { - ROS_ERROR("detected likely failure in snav, ensure it is running"); - continue; - } - else - { - nav_msgs::Odometry odom; - odom.header.stamp = ros::Time::now(); - odom.header.frame_id = "odom"; - - // set the position - odom.pose.pose.position.x = snav_data->vio_pos_vel.position_estimated[0]; - odom.pose.pose.position.y = snav_data->vio_pos_vel.position_estimated[1]; - odom.pose.pose.position.z = snav_data->vio_pos_vel.position_estimated[2]; - // set the attitude - roll = snav_data->attitude_estimate.roll; - pitch = snav_data->attitude_estimate.pitch; - yaw = snav_data->attitude_estimate.yaw; - // convert to quaternion - geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); - odom.pose.pose.orientation = odom_quat; - - // set the velocity - odom.child_frame_id = "base_link"; - odom.twist.twist.linear.x = snav_data->vio_pos_vel.velocity_estimated[0]; - odom.twist.twist.linear.y = snav_data->vio_pos_vel.velocity_estimated[1]; - odom.twist.twist.linear.z = snav_data->vio_pos_vel.velocity_estimated[2]; - - // publish the message - odom_pub.publish(odom); - - ros::spinOnce(); - loop_rate.sleep(); - } - } - return 0; -} \ No newline at end of file diff --git a/interfaces/kr_rosflight_interface/CMakeLists.txt b/interfaces/kr_rosflight_interface/CMakeLists.txt deleted file mode 100644 index e0800db2..00000000 --- a/interfaces/kr_rosflight_interface/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(kr_rosflight_interface) - -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) -endif() - -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) - -find_package( - catkin REQUIRED - COMPONENTS roscpp - nav_msgs - geometry_msgs - kr_mav_msgs - nodelet) -find_package(Eigen3 REQUIRED) - -# If rosflight_msgs not found, don't build but do not cause a build error -find_package(rosflight_msgs) - -if(NOT ${rosflight_msgs_FOUND}) - message(WARNING "NOTE: rosflight_msgs not found so not building kr_rosflight_interface") -else() - catkin_package( - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - roscpp - nav_msgs - geometry_msgs - kr_mav_msgs - rosflight_msgs - nodelet - DEPENDS - EIGEN3) - - add_library(${PROJECT_NAME} src/so3cmd_to_rosflight_nodelet.cpp) - target_include_directories(${PROJECT_NAME} PUBLIC ${caktin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} - ${rosflight_msgs_INCLUDE_DIRS}) - target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} Eigen3::Eigen) - add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - - install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - - install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -endif() diff --git a/interfaces/kr_rosflight_interface/README.md b/interfaces/kr_rosflight_interface/README.md deleted file mode 100644 index b6d4a7b0..00000000 --- a/interfaces/kr_rosflight_interface/README.md +++ /dev/null @@ -1,7 +0,0 @@ -# kr_rosflight_interface - -This node translates `kr_mav_msgs/SO3Command` to `rosflight_msgs/Command`. - -#### `rosflight_msgs` requirement - -This node requires `rosflight_msgs` to be present when building. If `rosflight_msgs` is not found when building the first time, a warning is given and nothing in this package is built. If `rosflight_msgs` is installed after this, force a recheck by adding `--force-cmake` to the `catkin_make`/`catkin build` command. diff --git a/interfaces/kr_rosflight_interface/launch/test.launch b/interfaces/kr_rosflight_interface/launch/test.launch deleted file mode 100644 index fb92879f..00000000 --- a/interfaces/kr_rosflight_interface/launch/test.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/interfaces/kr_rosflight_interface/nodelet_plugin.xml b/interfaces/kr_rosflight_interface/nodelet_plugin.xml deleted file mode 100644 index 512a2196..00000000 --- a/interfaces/kr_rosflight_interface/nodelet_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - This reformats an so3_command and publishes it on compatabile rosflight topics - - - diff --git a/interfaces/kr_rosflight_interface/package.xml b/interfaces/kr_rosflight_interface/package.xml deleted file mode 100644 index 67ac96e6..00000000 --- a/interfaces/kr_rosflight_interface/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - kr_rosflight_interface - 0.1.0 - kr_rosflight_interface - - Jon Fink - BSD - - - catkin - - - roscpp - nav_msgs - geometry_msgs - kr_mav_msgs - rosflight_msgs - nodelet - - - - - - diff --git a/interfaces/kr_rosflight_interface/src/so3cmd_to_rosflight_nodelet.cpp b/interfaces/kr_rosflight_interface/src/so3cmd_to_rosflight_nodelet.cpp deleted file mode 100644 index f9204ae0..00000000 --- a/interfaces/kr_rosflight_interface/src/so3cmd_to_rosflight_nodelet.cpp +++ /dev/null @@ -1,166 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -class SO3CmdToRosflight : public nodelet::Nodelet -{ - public: - void onInit(void); - - private: - void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg); - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - void imu_callback(const sensor_msgs::Imu::ConstPtr &pose); - - bool odom_set_, imu_set_, so3_cmd_set_; - Eigen::Quaterniond odom_q_, imu_q_; - int num_props_; - double max_prop_force_; - - ros::Publisher command_pub_; - - ros::Subscriber so3_cmd_sub_; - ros::Subscriber odom_sub_; - ros::Subscriber imu_sub_; - - double so3_cmd_timeout_; - ros::Time last_so3_cmd_time_; - kr_mav_msgs::SO3Command last_so3_cmd_; -}; - -void SO3CmdToRosflight::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) -{ - if(!odom_set_) - odom_set_ = true; - - odom_q_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, - odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); -} - -void SO3CmdToRosflight::imu_callback(const sensor_msgs::Imu::ConstPtr &pose) -{ - if(!imu_set_) - imu_set_ = true; - - imu_q_ = Eigen::Quaterniond(pose->orientation.w, pose->orientation.x, pose->orientation.y, pose->orientation.z); - - if(so3_cmd_set_ && ((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_)) - { - ROS_INFO("so3_cmd timeout. %f seconds since last command", (ros::Time::now() - last_so3_cmd_time_).toSec()); - const auto last_so3_cmd_ptr = boost::make_shared(last_so3_cmd_); - - so3_cmd_callback(last_so3_cmd_ptr); - } -} - -void SO3CmdToRosflight::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg) -{ - if(!so3_cmd_set_) - so3_cmd_set_ = true; - - // 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); - - tf::Quaternion q_des_tf = - tf::Quaternion(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w); - - // 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 - { - ROS_WARN_THROTTLE(1, "Psi %2.2f > 1.0, orientation error is too large!", Psi); - } - - double force = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); - - double throttle = force / (num_props_ * max_prop_force_); - - // 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 = boost::make_shared(); - setpoint_msg->header = msg->header; - setpoint_msg->mode = rosflight_msgs::Command::MODE_ROLL_PITCH_YAWRATE_THROTTLE; - setpoint_msg->ignore = rosflight_msgs::Command::IGNORE_NONE; - - double roll_sp, pitch_sp, yaw_sp; - tf::Matrix3x3(q_des_tf).getRPY(roll_sp, pitch_sp, yaw_sp); - - // Require conversion to FRD frame for rosflight - setpoint_msg->x = roll_sp; - setpoint_msg->y = -pitch_sp; - setpoint_msg->z = -msg->angular_velocity.z; // yaw rate - - // No field in rosflight_msgs/Command for rate setpoints at same time as attitude? - // setpoint_msg->mode = rosflight_msgs::Command::MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE; - // setpoint_msg->x = msg->angular_velocity.x; - // setpoint_msg->y = msg->angular_velocity.y; - // setpoint_msg->z = msg->angular_velocity.z; - - setpoint_msg->F = throttle; - - command_pub_.publish(setpoint_msg); - - // save last so3_cmd - last_so3_cmd_ = *msg; - last_so3_cmd_time_ = ros::Time::now(); -} - -void SO3CmdToRosflight::onInit(void) -{ - ros::NodeHandle priv_nh(getPrivateNodeHandle()); - - // get thrust scaling parameters - if(priv_nh.getParam("num_props", num_props_)) - ROS_INFO("Got number of props: %d", num_props_); - else - ROS_ERROR("Must set num_props param"); - - if(priv_nh.getParam("max_prop_force", max_prop_force_)) - ROS_INFO("Using max_prop_force=%g ", max_prop_force_); - else - ROS_ERROR("Must set max_prop_force param for thrust scaling"); - - ROS_ASSERT_MSG(max_prop_force_ > 0, "max_prop_force must be positive. max_prop_force = %g", max_prop_force_); - - // get param for so3 command timeout duration - priv_nh.param("so3_cmd_timeout", so3_cmd_timeout_, 0.25); - - odom_set_ = false; - imu_set_ = false; - so3_cmd_set_ = false; - - command_pub_ = priv_nh.advertise("command", 10); - - so3_cmd_sub_ = - priv_nh.subscribe("so3_cmd", 10, &SO3CmdToRosflight::so3_cmd_callback, this, ros::TransportHints().tcpNoDelay()); - - odom_sub_ = - priv_nh.subscribe("odom", 10, &SO3CmdToRosflight::odom_callback, this, ros::TransportHints().tcpNoDelay()); - - imu_sub_ = priv_nh.subscribe("imu", 10, &SO3CmdToRosflight::imu_callback, this, ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(SO3CmdToRosflight, nodelet::Nodelet); diff --git a/interfaces/kr_serial_interface/CMakeLists.txt b/interfaces/kr_serial_interface/CMakeLists.txt deleted file mode 100644 index 5753ed97..00000000 --- a/interfaces/kr_serial_interface/CMakeLists.txt +++ /dev/null @@ -1,48 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(kr_serial_interface) - -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) -endif() - -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) - -find_package(catkin REQUIRED COMPONENTS nodelet kr_mav_msgs roscpp sensor_msgs) - -include_directories(include ${catkin_INCLUDE_DIRS}) - -catkin_package( - INCLUDE_DIRS - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - nodelet - kr_mav_msgs - roscpp - sensor_msgs - DEPENDS) - -add_library( - ${PROJECT_NAME} - src/ASIOSerialDevice.cc - src/asctec_serial_interface.cpp - src/decode_msgs.cpp - src/encode_msgs.cpp - src/quad_decode_msg_nodelet.cpp - src/quad_encode_msg.cpp - src/quad_encode_msg_nodelet.cpp - src/quad_serial_comm_nodelet.cpp) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/interfaces/kr_serial_interface/LICENSE b/interfaces/kr_serial_interface/LICENSE deleted file mode 100644 index 94a9ed02..00000000 --- a/interfaces/kr_serial_interface/LICENSE +++ /dev/null @@ -1,674 +0,0 @@ - GNU GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The GNU General Public License is a free, copyleft license for -software and other kinds of works. - - The licenses for most software and other practical works are designed -to take away your freedom to share and change the works. By contrast, -the GNU General Public License is intended to guarantee your freedom to -share and change all versions of a program--to make sure it remains free -software for all its users. We, the Free Software Foundation, use the -GNU General Public License for most of our software; it applies also to -any other work released this way by its authors. You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -them if you wish), that you receive source code or can get it if you -want it, that you can change the software or use pieces of it in new -free programs, and that you know you can do these things. - - To protect your rights, we need to prevent others from denying you -these rights or asking you to surrender the rights. Therefore, you have -certain responsibilities if you distribute copies of the software, or if -you modify it: responsibilities to respect the freedom of others. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must pass on to the recipients the same -freedoms that you received. You must make sure that they, too, receive -or can get the source code. And you must show them these terms so they -know their rights. - - Developers that use the GNU GPL protect your rights with two steps: -(1) assert copyright on the software, and (2) offer you this License -giving you legal permission to copy, distribute and/or modify it. - - For the developers' and authors' protection, the GPL clearly explains -that there is no warranty for this free software. For both users' and -authors' sake, the GPL requires that modified versions be marked as -changed, so that their problems will not be attributed erroneously to -authors of previous versions. - - Some devices are designed to deny users access to install or run -modified versions of the software inside them, although the manufacturer -can do so. This is fundamentally incompatible with the aim of -protecting users' freedom to change the software. The systematic -pattern of such abuse occurs in the area of products for individuals to -use, which is precisely where it is most unacceptable. Therefore, we -have designed this version of the GPL to prohibit the practice for those -products. If such problems arise substantially in other domains, we -stand ready to extend this provision to those domains in future versions -of the GPL, as needed to protect the freedom of users. - - Finally, every program is threatened constantly by software patents. -States should not allow patents to restrict development and use of -software on general-purpose computers, but in those that do, we wish to -avoid the special danger that patents applied to a free program could -make it effectively proprietary. To prevent this, the GPL assures that -patents cannot be used to render the program non-free. - - The precise terms and conditions for copying, distribution and -modification follow. - - TERMS AND CONDITIONS - - 0. Definitions. - - "This License" refers to version 3 of the GNU General Public License. - - "Copyright" also means copyright-like laws that apply to other kinds of -works, such as semiconductor masks. - - "The Program" refers to any copyrightable work licensed under this -License. Each licensee is addressed as "you". "Licensees" and -"recipients" may be individuals or organizations. - - To "modify" a work means to copy from or adapt all or part of the work -in a fashion requiring copyright permission, other than the making of an -exact copy. The resulting work is called a "modified version" of the -earlier work or a work "based on" the earlier work. - - A "covered work" means either the unmodified Program or a work based -on the Program. - - To "propagate" a work means to do anything with it that, without -permission, would make you directly or secondarily liable for -infringement under applicable copyright law, except executing it on a -computer or modifying a private copy. Propagation includes copying, -distribution (with or without modification), making available to the -public, and in some countries other activities as well. - - To "convey" a work means any kind of propagation that enables other -parties to make or receive copies. Mere interaction with a user through -a computer network, with no transfer of a copy, is not conveying. - - An interactive user interface displays "Appropriate Legal Notices" -to the extent that it includes a convenient and prominently visible -feature that (1) displays an appropriate copyright notice, and (2) -tells the user that there is no warranty for the work (except to the -extent that warranties are provided), that licensees may convey the -work under this License, and how to view a copy of this License. If -the interface presents a list of user commands or options, such as a -menu, a prominent item in the list meets this criterion. - - 1. Source Code. - - The "source code" for a work means the preferred form of the work -for making modifications to it. "Object code" means any non-source -form of a work. - - A "Standard Interface" means an interface that either is an official -standard defined by a recognized standards body, or, in the case of -interfaces specified for a particular programming language, one that -is widely used among developers working in that language. - - The "System Libraries" of an executable work include anything, other -than the work as a whole, that (a) is included in the normal form of -packaging a Major Component, but which is not part of that Major -Component, and (b) serves only to enable use of the work with that -Major Component, or to implement a Standard Interface for which an -implementation is available to the public in source code form. A -"Major Component", in this context, means a major essential component -(kernel, window system, and so on) of the specific operating system -(if any) on which the executable work runs, or a compiler used to -produce the work, or an object code interpreter used to run it. - - The "Corresponding Source" for a work in object code form means all -the source code needed to generate, install, and (for an executable -work) run the object code and to modify the work, including scripts to -control those activities. However, it does not include the work's -System Libraries, or general-purpose tools or generally available free -programs which are used unmodified in performing those activities but -which are not part of the work. For example, Corresponding Source -includes interface definition files associated with source files for -the work, and the source code for shared libraries and dynamically -linked subprograms that the work is specifically designed to require, -such as by intimate data communication or control flow between those -subprograms and other parts of the work. - - The Corresponding Source need not include anything that users -can regenerate automatically from other parts of the Corresponding -Source. - - The Corresponding Source for a work in source code form is that -same work. - - 2. Basic Permissions. - - All rights granted under this License are granted for the term of -copyright on the Program, and are irrevocable provided the stated -conditions are met. This License explicitly affirms your unlimited -permission to run the unmodified Program. The output from running a -covered work is covered by this License only if the output, given its -content, constitutes a covered work. This License acknowledges your -rights of fair use or other equivalent, as provided by copyright law. - - You may make, run and propagate covered works that you do not -convey, without conditions so long as your license otherwise remains -in force. You may convey covered works to others for the sole purpose -of having them make modifications exclusively for you, or provide you -with facilities for running those works, provided that you comply with -the terms of this License in conveying all material for which you do -not control copyright. Those thus making or running the covered works -for you must do so exclusively on your behalf, under your direction -and control, on terms that prohibit them from making any copies of -your copyrighted material outside their relationship with you. - - Conveying under any other circumstances is permitted solely under -the conditions stated below. Sublicensing is not allowed; section 10 -makes it unnecessary. - - 3. Protecting Users' Legal Rights From Anti-Circumvention Law. - - No covered work shall be deemed part of an effective technological -measure under any applicable law fulfilling obligations under article -11 of the WIPO copyright treaty adopted on 20 December 1996, or -similar laws prohibiting or restricting circumvention of such -measures. - - When you convey a covered work, you waive any legal power to forbid -circumvention of technological measures to the extent such circumvention -is effected by exercising rights under this License with respect to -the covered work, and you disclaim any intention to limit operation or -modification of the work as a means of enforcing, against the work's -users, your or third parties' legal rights to forbid circumvention of -technological measures. - - 4. Conveying Verbatim Copies. - - You may convey verbatim copies of the Program's source code as you -receive it, in any medium, provided that you conspicuously and -appropriately publish on each copy an appropriate copyright notice; -keep intact all notices stating that this License and any -non-permissive terms added in accord with section 7 apply to the code; -keep intact all notices of the absence of any warranty; and give all -recipients a copy of this License along with the Program. - - You may charge any price or no price for each copy that you convey, -and you may offer support or warranty protection for a fee. - - 5. Conveying Modified Source Versions. - - You may convey a work based on the Program, or the modifications to -produce it from the Program, in the form of source code under the -terms of section 4, provided that you also meet all of these conditions: - - a) The work must carry prominent notices stating that you modified - it, and giving a relevant date. - - b) The work must carry prominent notices stating that it is - released under this License and any conditions added under section - 7. This requirement modifies the requirement in section 4 to - "keep intact all notices". - - c) You must license the entire work, as a whole, under this - License to anyone who comes into possession of a copy. This - License will therefore apply, along with any applicable section 7 - additional terms, to the whole of the work, and all its parts, - regardless of how they are packaged. This License gives no - permission to license the work in any other way, but it does not - invalidate such permission if you have separately received it. - - d) If the work has interactive user interfaces, each must display - Appropriate Legal Notices; however, if the Program has interactive - interfaces that do not display Appropriate Legal Notices, your - work need not make them do so. - - A compilation of a covered work with other separate and independent -works, which are not by their nature extensions of the covered work, -and which are not combined with it such as to form a larger program, -in or on a volume of a storage or distribution medium, is called an -"aggregate" if the compilation and its resulting copyright are not -used to limit the access or legal rights of the compilation's users -beyond what the individual works permit. Inclusion of a covered work -in an aggregate does not cause this License to apply to the other -parts of the aggregate. - - 6. Conveying Non-Source Forms. - - You may convey a covered work in object code form under the terms -of sections 4 and 5, provided that you also convey the -machine-readable Corresponding Source under the terms of this License, -in one of these ways: - - a) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by the - Corresponding Source fixed on a durable physical medium - customarily used for software interchange. - - b) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by a - written offer, valid for at least three years and valid for as - long as you offer spare parts or customer support for that product - model, to give anyone who possesses the object code either (1) a - copy of the Corresponding Source for all the software in the - product that is covered by this License, on a durable physical - medium customarily used for software interchange, for a price no - more than your reasonable cost of physically performing this - conveying of source, or (2) access to copy the - Corresponding Source from a network server at no charge. - - c) Convey individual copies of the object code with a copy of the - written offer to provide the Corresponding Source. This - alternative is allowed only occasionally and noncommercially, and - only if you received the object code with such an offer, in accord - with subsection 6b. - - d) Convey the object code by offering access from a designated - place (gratis or for a charge), and offer equivalent access to the - Corresponding Source in the same way through the same place at no - further charge. You need not require recipients to copy the - Corresponding Source along with the object code. If the place to - copy the object code is a network server, the Corresponding Source - may be on a different server (operated by you or a third party) - that supports equivalent copying facilities, provided you maintain - clear directions next to the object code saying where to find the - Corresponding Source. Regardless of what server hosts the - Corresponding Source, you remain obligated to ensure that it is - available for as long as needed to satisfy these requirements. - - e) Convey the object code using peer-to-peer transmission, provided - you inform other peers where the object code and Corresponding - Source of the work are being offered to the general public at no - charge under subsection 6d. - - A separable portion of the object code, whose source code is excluded -from the Corresponding Source as a System Library, need not be -included in conveying the object code work. - - A "User Product" is either (1) a "consumer product", which means any -tangible personal property which is normally used for personal, family, -or household purposes, or (2) anything designed or sold for incorporation -into a dwelling. In determining whether a product is a consumer product, -doubtful cases shall be resolved in favor of coverage. For a particular -product received by a particular user, "normally used" refers to a -typical or common use of that class of product, regardless of the status -of the particular user or of the way in which the particular user -actually uses, or expects or is expected to use, the product. A product -is a consumer product regardless of whether the product has substantial -commercial, industrial or non-consumer uses, unless such uses represent -the only significant mode of use of the product. - - "Installation Information" for a User Product means any methods, -procedures, authorization keys, or other information required to install -and execute modified versions of a covered work in that User Product from -a modified version of its Corresponding Source. The information must -suffice to ensure that the continued functioning of the modified object -code is in no case prevented or interfered with solely because -modification has been made. - - If you convey an object code work under this section in, or with, or -specifically for use in, a User Product, and the conveying occurs as -part of a transaction in which the right of possession and use of the -User Product is transferred to the recipient in perpetuity or for a -fixed term (regardless of how the transaction is characterized), the -Corresponding Source conveyed under this section must be accompanied -by the Installation Information. But this requirement does not apply -if neither you nor any third party retains the ability to install -modified object code on the User Product (for example, the work has -been installed in ROM). - - The requirement to provide Installation Information does not include a -requirement to continue to provide support service, warranty, or updates -for a work that has been modified or installed by the recipient, or for -the User Product in which it has been modified or installed. Access to a -network may be denied when the modification itself materially and -adversely affects the operation of the network or violates the rules and -protocols for communication across the network. - - Corresponding Source conveyed, and Installation Information provided, -in accord with this section must be in a format that is publicly -documented (and with an implementation available to the public in -source code form), and must require no special password or key for -unpacking, reading or copying. - - 7. Additional Terms. - - "Additional permissions" are terms that supplement the terms of this -License by making exceptions from one or more of its conditions. -Additional permissions that are applicable to the entire Program shall -be treated as though they were included in this License, to the extent -that they are valid under applicable law. If additional permissions -apply only to part of the Program, that part may be used separately -under those permissions, but the entire Program remains governed by -this License without regard to the additional permissions. - - When you convey a copy of a covered work, you may at your option -remove any additional permissions from that copy, or from any part of -it. (Additional permissions may be written to require their own -removal in certain cases when you modify the work.) You may place -additional permissions on material, added by you to a covered work, -for which you have or can give appropriate copyright permission. - - Notwithstanding any other provision of this License, for material you -add to a covered work, you may (if authorized by the copyright holders of -that material) supplement the terms of this License with terms: - - a) Disclaiming warranty or limiting liability differently from the - terms of sections 15 and 16 of this License; or - - b) Requiring preservation of specified reasonable legal notices or - author attributions in that material or in the Appropriate Legal - Notices displayed by works containing it; or - - c) Prohibiting misrepresentation of the origin of that material, or - requiring that modified versions of such material be marked in - reasonable ways as different from the original version; or - - d) Limiting the use for publicity purposes of names of licensors or - authors of the material; or - - e) Declining to grant rights under trademark law for use of some - trade names, trademarks, or service marks; or - - f) Requiring indemnification of licensors and authors of that - material by anyone who conveys the material (or modified versions of - it) with contractual assumptions of liability to the recipient, for - any liability that these contractual assumptions directly impose on - those licensors and authors. - - All other non-permissive additional terms are considered "further -restrictions" within the meaning of section 10. If the Program as you -received it, or any part of it, contains a notice stating that it is -governed by this License along with a term that is a further -restriction, you may remove that term. If a license document contains -a further restriction but permits relicensing or conveying under this -License, you may add to a covered work material governed by the terms -of that license document, provided that the further restriction does -not survive such relicensing or conveying. - - If you add terms to a covered work in accord with this section, you -must place, in the relevant source files, a statement of the -additional terms that apply to those files, or a notice indicating -where to find the applicable terms. - - Additional terms, permissive or non-permissive, may be stated in the -form of a separately written license, or stated as exceptions; -the above requirements apply either way. - - 8. Termination. - - You may not propagate or modify a covered work except as expressly -provided under this License. Any attempt otherwise to propagate or -modify it is void, and will automatically terminate your rights under -this License (including any patent licenses granted under the third -paragraph of section 11). - - However, if you cease all violation of this License, then your -license from a particular copyright holder is reinstated (a) -provisionally, unless and until the copyright holder explicitly and -finally terminates your license, and (b) permanently, if the copyright -holder fails to notify you of the violation by some reasonable means -prior to 60 days after the cessation. - - Moreover, your license from a particular copyright holder is -reinstated permanently if the copyright holder notifies you of the -violation by some reasonable means, this is the first time you have -received notice of violation of this License (for any work) from that -copyright holder, and you cure the violation prior to 30 days after -your receipt of the notice. - - Termination of your rights under this section does not terminate the -licenses of parties who have received copies or rights from you under -this License. If your rights have been terminated and not permanently -reinstated, you do not qualify to receive new licenses for the same -material under section 10. - - 9. Acceptance Not Required for Having Copies. - - You are not required to accept this License in order to receive or -run a copy of the Program. Ancillary propagation of a covered work -occurring solely as a consequence of using peer-to-peer transmission -to receive a copy likewise does not require acceptance. However, -nothing other than this License grants you permission to propagate or -modify any covered work. These actions infringe copyright if you do -not accept this License. Therefore, by modifying or propagating a -covered work, you indicate your acceptance of this License to do so. - - 10. Automatic Licensing of Downstream Recipients. - - Each time you convey a covered work, the recipient automatically -receives a license from the original licensors, to run, modify and -propagate that work, subject to this License. You are not responsible -for enforcing compliance by third parties with this License. - - An "entity transaction" is a transaction transferring control of an -organization, or substantially all assets of one, or subdividing an -organization, or merging organizations. If propagation of a covered -work results from an entity transaction, each party to that -transaction who receives a copy of the work also receives whatever -licenses to the work the party's predecessor in interest had or could -give under the previous paragraph, plus a right to possession of the -Corresponding Source of the work from the predecessor in interest, if -the predecessor has it or can get it with reasonable efforts. - - You may not impose any further restrictions on the exercise of the -rights granted or affirmed under this License. For example, you may -not impose a license fee, royalty, or other charge for exercise of -rights granted under this License, and you may not initiate litigation -(including a cross-claim or counterclaim in a lawsuit) alleging that -any patent claim is infringed by making, using, selling, offering for -sale, or importing the Program or any portion of it. - - 11. Patents. - - A "contributor" is a copyright holder who authorizes use under this -License of the Program or a work on which the Program is based. The -work thus licensed is called the contributor's "contributor version". - - A contributor's "essential patent claims" are all patent claims -owned or controlled by the contributor, whether already acquired or -hereafter acquired, that would be infringed by some manner, permitted -by this License, of making, using, or selling its contributor version, -but do not include claims that would be infringed only as a -consequence of further modification of the contributor version. For -purposes of this definition, "control" includes the right to grant -patent sublicenses in a manner consistent with the requirements of -this License. - - Each contributor grants you a non-exclusive, worldwide, royalty-free -patent license under the contributor's essential patent claims, to -make, use, sell, offer for sale, import and otherwise run, modify and -propagate the contents of its contributor version. - - In the following three paragraphs, a "patent license" is any express -agreement or commitment, however denominated, not to enforce a patent -(such as an express permission to practice a patent or covenant not to -sue for patent infringement). To "grant" such a patent license to a -party means to make such an agreement or commitment not to enforce a -patent against the party. - - If you convey a covered work, knowingly relying on a patent license, -and the Corresponding Source of the work is not available for anyone -to copy, free of charge and under the terms of this License, through a -publicly available network server or other readily accessible means, -then you must either (1) cause the Corresponding Source to be so -available, or (2) arrange to deprive yourself of the benefit of the -patent license for this particular work, or (3) arrange, in a manner -consistent with the requirements of this License, to extend the patent -license to downstream recipients. "Knowingly relying" means you have -actual knowledge that, but for the patent license, your conveying the -covered work in a country, or your recipient's use of the covered work -in a country, would infringe one or more identifiable patents in that -country that you have reason to believe are valid. - - If, pursuant to or in connection with a single transaction or -arrangement, you convey, or propagate by procuring conveyance of, a -covered work, and grant a patent license to some of the parties -receiving the covered work authorizing them to use, propagate, modify -or convey a specific copy of the covered work, then the patent license -you grant is automatically extended to all recipients of the covered -work and works based on it. - - A patent license is "discriminatory" if it does not include within -the scope of its coverage, prohibits the exercise of, or is -conditioned on the non-exercise of one or more of the rights that are -specifically granted under this License. You may not convey a covered -work if you are a party to an arrangement with a third party that is -in the business of distributing software, under which you make payment -to the third party based on the extent of your activity of conveying -the work, and under which the third party grants, to any of the -parties who would receive the covered work from you, a discriminatory -patent license (a) in connection with copies of the covered work -conveyed by you (or copies made from those copies), or (b) primarily -for and in connection with specific products or compilations that -contain the covered work, unless you entered into that arrangement, -or that patent license was granted, prior to 28 March 2007. - - Nothing in this License shall be construed as excluding or limiting -any implied license or other defenses to infringement that may -otherwise be available to you under applicable patent law. - - 12. No Surrender of Others' Freedom. - - If conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot convey a -covered work so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you may -not convey it at all. For example, if you agree to terms that obligate you -to collect a royalty for further conveying from those to whom you convey -the Program, the only way you could satisfy both those terms and this -License would be to refrain entirely from conveying the Program. - - 13. Use with the GNU Affero General Public License. - - Notwithstanding any other provision of this License, you have -permission to link or combine any covered work with a work licensed -under version 3 of the GNU Affero General Public License into a single -combined work, and to convey the resulting work. The terms of this -License will continue to apply to the part which is the covered work, -but the special requirements of the GNU Affero General Public License, -section 13, concerning interaction through a network will apply to the -combination as such. - - 14. Revised Versions of this License. - - The Free Software Foundation may publish revised and/or new versions of -the GNU General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - - Each version is given a distinguishing version number. If the -Program specifies that a certain numbered version of the GNU General -Public License "or any later version" applies to it, you have the -option of following the terms and conditions either of that numbered -version or of any later version published by the Free Software -Foundation. If the Program does not specify a version number of the -GNU General Public License, you may choose any version ever published -by the Free Software Foundation. - - If the Program specifies that a proxy can decide which future -versions of the GNU General Public License can be used, that proxy's -public statement of acceptance of a version permanently authorizes you -to choose that version for the Program. - - Later license versions may give you additional or different -permissions. However, no additional obligations are imposed on any -author or copyright holder as a result of your choosing to follow a -later version. - - 15. Disclaimer of Warranty. - - THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY -APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT -HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY -OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM -IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF -ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. Limitation of Liability. - - IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS -THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY -GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE -USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF -DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD -PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), -EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF -SUCH DAMAGES. - - 17. Interpretation of Sections 15 and 16. - - If the disclaimer of warranty and limitation of liability provided -above cannot be given local legal effect according to their terms, -reviewing courts shall apply local law that most closely approximates -an absolute waiver of all civil liability in connection with the -Program, unless a warranty or assumption of liability accompanies a -copy of the Program in return for a fee. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -state the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - -Also add information on how to contact you by electronic and paper mail. - - If the program does terminal interaction, make it output a short -notice like this when it starts in an interactive mode: - - Copyright (C) - This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, your program's commands -might be different; for a GUI interface, you would use an "about box". - - You should also get your employer (if you work as a programmer) or school, -if any, to sign a "copyright disclaimer" for the program, if necessary. -For more information on this, and how to apply and follow the GNU GPL, see -. - - The GNU General Public License does not permit incorporating your program -into proprietary programs. If your program is a subroutine library, you -may consider it more useful to permit linking proprietary applications with -the library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. But first, please read -. diff --git a/interfaces/kr_serial_interface/include/kr_serial_interface/ASIOSerialDevice.h b/interfaces/kr_serial_interface/include/kr_serial_interface/ASIOSerialDevice.h deleted file mode 100644 index e18572d6..00000000 --- a/interfaces/kr_serial_interface/include/kr_serial_interface/ASIOSerialDevice.h +++ /dev/null @@ -1,85 +0,0 @@ -/* - This file is part of asio_serial_device, a class wrapper to - use the boost::asio serial functionality. - - asio_serial_device is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - - Nathan Michael, Aug. 2011 -*/ - -#ifndef ASIOSERIALDEVICE_H -#define ASIOSERIALDEVICE_H - -#include -#include -#include -#include -#include -#include - -// Class version of example ASIO over serial with boost::asio: -// http://groups.google.com/group/boost-list/browse_thread/thread/5cc7dcc7b90d41fc - -#define MAX_READ_LENGTH 16 - -namespace ba = boost::asio; - -class ASIOSerialDevice -{ - public: - ASIOSerialDevice(); - ASIOSerialDevice(const std::string &device, unsigned int baud); - ~ASIOSerialDevice(); - - void SetReadCallback(const boost::function &handler); - - void Start(); - void Stop(); - void Close(); - - void Read(); - bool Write(const std::vector &msg); - void Open( - const std::string &device_, unsigned int baud_, - ba::serial_port_base::parity parity = ba::serial_port_base::parity(ba::serial_port_base::parity::none), - ba::serial_port_base::character_size csize = ba::serial_port_base::character_size(8), - ba::serial_port_base::flow_control flow = - ba::serial_port_base::flow_control(ba::serial_port_base::flow_control::none), - ba::serial_port_base::stop_bits stop = ba::serial_port_base::stop_bits(ba::serial_port_base::stop_bits::one)); - - bool Active(); - - private: - void CloseCallback(const boost::system::error_code &error); - - void ReadStart(); - void ReadComplete(const boost::system::error_code &error, size_t bytes_transferred); - - void WriteCallback(const std::vector &msg); - void WriteStart(); - void WriteComplete(const boost::system::error_code &error); - - std::string device; - unsigned int baud; - bool async_active, open; - std::deque > write_msgs; - - boost::thread thread; - ba::io_service io_service; - ba::serial_port *serial_port; - boost::function read_callback; - - unsigned char read_msg[MAX_READ_LENGTH]; -}; -#endif diff --git a/interfaces/kr_serial_interface/include/kr_serial_interface/comm_types.h b/interfaces/kr_serial_interface/include/kr_serial_interface/comm_types.h deleted file mode 100644 index 01c5558b..00000000 --- a/interfaces/kr_serial_interface/include/kr_serial_interface/comm_types.h +++ /dev/null @@ -1,66 +0,0 @@ -#ifndef QUADROTOR_MSGS_COMM_TYPES_H -#define QUADROTOR_MSGS_COMM_TYPES_H - -#include - -#define TYPE_SO3_CMD 's' -struct SO3_CMD_INPUT -{ - // Scaling factors when decoding - int16_t force[3]; // /500 (range: +-65 N on each axis) - int8_t des_qx, des_qy, des_qz, des_qw; // /125 - int16_t angvel_x, angvel_y, angvel_z; // /1000 (range: +-32 rad/s) - uint8_t kR[3]; // /50 (range: 0-5.1) - uint8_t kOm[3]; // /100 (range: 0-2.5) - int16_t cur_yaw; // /1e4 - int16_t kf_correction; // /1e11 (range: +-3.2e-7) - int8_t angle_corrections[2]; // roll,pitch /2500 (range: +-0.05 rad) - uint8_t enable_motors : 1; - uint8_t use_external_yaw : 1; - uint8_t seq; -}; - -#define TYPE_STATUS_DATA 'c' -struct STATUS_DATA -{ - uint16_t loop_rate; - uint16_t voltage; - uint8_t seq; -}; - -#define TYPE_OUTPUT_DATA 'd' -struct OUTPUT_DATA -{ - uint16_t loop_rate; - uint16_t voltage; - int16_t roll, pitch, yaw; - int16_t ang_vel[3]; - int16_t acc[3]; - int16_t dheight; - int32_t height; - int16_t mag[3]; - uint8_t radio[8]; - uint8_t rpm[4]; - uint8_t seq; -}; - -#define TYPE_TRPY_CMD 'p' -struct TRPY_CMD -{ - int16_t thrust; - int16_t roll; - int16_t pitch; - int16_t yaw; - int16_t current_yaw; - uint8_t enable_motors : 1; - uint8_t use_external_yaw : 1; -}; - -#define TYPE_PWM_CMD 'w' -struct PWM_CMD_INPUT -{ - // Scaling factors when decoding - uint8_t pwm[2]; // /255 -}; - -#endif diff --git a/interfaces/kr_serial_interface/include/kr_serial_interface/decode_msgs.h b/interfaces/kr_serial_interface/include/kr_serial_interface/decode_msgs.h deleted file mode 100644 index 9d049a28..00000000 --- a/interfaces/kr_serial_interface/include/kr_serial_interface/decode_msgs.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef QUADROTOR_MSGS_DECODE_MSGS_H -#define QUADROTOR_MSGS_DECODE_MSGS_H - -#include -#include -#include - -#include - -namespace kr_mav_msgs -{ -bool decodeOutputData(const std::vector &data, kr_mav_msgs::OutputData &output); - -bool decodeStatusData(const std::vector &data, kr_mav_msgs::StatusData &status); -} // namespace kr_mav_msgs - -#endif diff --git a/interfaces/kr_serial_interface/include/kr_serial_interface/encode_msgs.h b/interfaces/kr_serial_interface/include/kr_serial_interface/encode_msgs.h deleted file mode 100644 index f5e8bc4a..00000000 --- a/interfaces/kr_serial_interface/include/kr_serial_interface/encode_msgs.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef QUADROTOR_MSGS_ENCODE_MSGS_H -#define QUADROTOR_MSGS_ENCODE_MSGS_H - -#include -#include -#include -#include - -#include - -namespace kr_mav_msgs -{ -void encodeSO3Command(const kr_mav_msgs::SO3Command &so3_command, std::vector &output); -void encodeTRPYCommand(const kr_mav_msgs::TRPYCommand &trpy_command, std::vector &output); -void encodePWMCommand(const kr_mav_msgs::PWMCommand &pwm_command, std::vector &output); -} // namespace kr_mav_msgs - -#endif diff --git a/interfaces/kr_serial_interface/include/kr_serial_interface/serial_interface.h b/interfaces/kr_serial_interface/include/kr_serial_interface/serial_interface.h deleted file mode 100644 index 591e8773..00000000 --- a/interfaces/kr_serial_interface/include/kr_serial_interface/serial_interface.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef QUAD_SERIAL_COMM_SERIAL_INTERFACE_H -#define QUAD_SERIAL_COMM_SERIAL_INTERFACE_H - -#include - -#include -#include - -void encode_serial_msg(const kr_mav_msgs::Serial &msg, std::vector &serial_data); - -void process_serial_data(const uint8_t *data, const size_t count, - boost::function callback); -#endif diff --git a/interfaces/kr_serial_interface/launch/test.launch b/interfaces/kr_serial_interface/launch/test.launch deleted file mode 100644 index ef361f39..00000000 --- a/interfaces/kr_serial_interface/launch/test.launch +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/interfaces/kr_serial_interface/nodelet_plugin.xml b/interfaces/kr_serial_interface/nodelet_plugin.xml deleted file mode 100644 index 6a36c83e..00000000 --- a/interfaces/kr_serial_interface/nodelet_plugin.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - This packs the data being sent to the quad into a nice efficient byte-stream. - - - - - - This unpacks the data being sent from the quad into a ROS message. - - - - - - This sends the packed data over the serial port. It also receives data from the quad and unpacks the payload from the received packet. - - - diff --git a/interfaces/kr_serial_interface/package.xml b/interfaces/kr_serial_interface/package.xml deleted file mode 100644 index e5e42b4e..00000000 --- a/interfaces/kr_serial_interface/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - kr_serial_interface - 1.0.0 - kr_serial_interface - GPL license since asio_serial_device is under GPL and we link to it. - Kartik Mohta - Michael Watterson - - GPL-3.0 - - Kartik Mohta - - - catkin - - - nodelet - kr_mav_msgs - roscpp - sensor_msgs - - - - - - diff --git a/interfaces/kr_serial_interface/src/ASIOSerialDevice.cc b/interfaces/kr_serial_interface/src/ASIOSerialDevice.cc deleted file mode 100644 index d3a4c289..00000000 --- a/interfaces/kr_serial_interface/src/ASIOSerialDevice.cc +++ /dev/null @@ -1,238 +0,0 @@ -/* - This file is part of asio_serial_device, a class wrapper to - use the boost::asio serial functionality. - - asio_serial_device is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - - Nathan Michael, Aug. 2011 -*/ - -#include - -#if defined(__linux__) -#include -#endif - -using namespace std; - -ASIOSerialDevice::ASIOSerialDevice() -{ - async_active = false; - open = false; - serial_port = 0; -} - -ASIOSerialDevice::ASIOSerialDevice(const string &device, unsigned int baud) -{ - async_active = false; - open = false; - serial_port = 0; - - Open(device, baud); -} - -ASIOSerialDevice::~ASIOSerialDevice() -{ - if(open) - Close(); - - if(async_active) - Stop(); - - if(serial_port != 0) - delete serial_port; -} - -void ASIOSerialDevice::Open(const string &device_, unsigned int baud_, ba::serial_port_base::parity parity, - ba::serial_port_base::character_size csize, ba::serial_port_base::flow_control flow, - ba::serial_port_base::stop_bits stop) -{ - device = device_; - baud = baud_; - - if(!open) - { - try - { - serial_port = new ba::serial_port(io_service, device); - } - catch(const std::exception &e) - { - cerr << "Unable to open device: " << device << endl; - throw; - } - - if(!serial_port->is_open()) - throw runtime_error("Failed to open serial port"); - - ba::serial_port_base::baud_rate baud(baud_); - serial_port->set_option(baud); - - serial_port->set_option(parity); - serial_port->set_option(csize); - serial_port->set_option(flow); - serial_port->set_option(stop); - -#if defined(__linux__) - int fd = serial_port->native_handle(); - - // Enable low latency mode on Linux - struct serial_struct ser_info; - ioctl(fd, TIOCGSERIAL, &ser_info); - ser_info.flags |= ASYNC_LOW_LATENCY; - ioctl(fd, TIOCSSERIAL, &ser_info); - -#if BOOST_ASIO_VERSION < 101200 - // This is done in Boost.ASIO, but until v1.12.0 (Boost 1.66) there was a - // bug which doesn't enable relevant code. Fixed by commit: - // https://github.com/boostorg/asio/commit/619cea4356 - termios tio; - tcgetattr(fd, &tio); - - // Set serial port to "raw" mode to prevent EOF exit. - cfmakeraw(&tio); - - // Commit settings - tcsetattr(fd, TCSANOW, &tio); -#endif -#endif - - open = true; - } -} - -void ASIOSerialDevice::Close() -{ - if(open) - { - if(async_active) - io_service.post(boost::bind(&ASIOSerialDevice::CloseCallback, this, boost::system::error_code())); - else - CloseCallback(boost::system::error_code()); - } -} - -void ASIOSerialDevice::Start() -{ - if(!open) - throw runtime_error("Serial port interface not open"); - - ReadStart(); - - thread = boost::thread(boost::bind(&ba::io_service::run, &io_service)); - - async_active = true; - - return; -} - -void ASIOSerialDevice::ReadStart() -{ - if(open) - serial_port->async_read_some(ba::buffer(read_msg, MAX_READ_LENGTH), - boost::bind(&ASIOSerialDevice::ReadComplete, this, ba::placeholders::error, - ba::placeholders::bytes_transferred)); -} - -void ASIOSerialDevice::ReadComplete(const boost::system::error_code &error, size_t bytes_transferred) -{ - if(!error) - { - if(!read_callback.empty()) - read_callback(read_msg, bytes_transferred); - ReadStart(); - } - else - CloseCallback(error); -} - -void ASIOSerialDevice::SetReadCallback(const boost::function &handler) -{ - read_callback = handler; -} - -void ASIOSerialDevice::Stop() -{ - thread.join(); - async_active = false; -} - -void ASIOSerialDevice::CloseCallback(const boost::system::error_code &error) -{ - if(error && (error != ba::error::operation_aborted)) - cerr << "Error: " << error.message() << endl; - - serial_port->close(); - open = false; -} - -bool ASIOSerialDevice::Write(const vector &msg) -{ - if(!open) - return false; - - if(async_active) - // Post for an asynchronous write - io_service.post(boost::bind(&ASIOSerialDevice::WriteCallback, this, msg)); - else - // Write synchronously - ba::write(*serial_port, ba::buffer(&(msg[0]), msg.size())); - - return true; -} - -void ASIOSerialDevice::WriteCallback(const vector &msg) -{ - bool write_in_progress = !write_msgs.empty(); - write_msgs.push_back(msg); - if(!write_in_progress) - WriteStart(); -} - -void ASIOSerialDevice::WriteStart() -{ - ba::async_write(*serial_port, ba::buffer(&(write_msgs.front()[0]), write_msgs.front().size()), - boost::bind(&ASIOSerialDevice::WriteComplete, this, ba::placeholders::error)); -} - -void ASIOSerialDevice::WriteComplete(const boost::system::error_code &error) -{ - if(!error) - { - write_msgs.pop_front(); - if(!write_msgs.empty()) - WriteStart(); - } - else - CloseCallback(error); -} - -bool ASIOSerialDevice::Active() -{ - return async_active; -} - -void ASIOSerialDevice::Read() -{ - if(async_active) - { - cerr << "ASIOSerialDevice can operate in async or sync modes, not both" << endl; - return; - } - - size_t bytes_transferred = serial_port->read_some(ba::buffer(read_msg, MAX_READ_LENGTH)); - - if(!read_callback.empty() && (bytes_transferred > 0)) - read_callback(read_msg, bytes_transferred); -} diff --git a/interfaces/kr_serial_interface/src/asctec_serial_interface.cpp b/interfaces/kr_serial_interface/src/asctec_serial_interface.cpp deleted file mode 100644 index 850feb70..00000000 --- a/interfaces/kr_serial_interface/src/asctec_serial_interface.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include - -/* Packet format: - * Packet: | 0x55 | 0x55 |count|type|---data---|crcL|crcH| - * State: 0 1 2 3 4 5 6 - */ - -enum PacketState -{ - kPacketStart1, - kPacketStart2, - kPacketCount, - kPacketType, - kPacketData, - kPacketCRCL, - kPacketCRCH, -}; - -static const unsigned int kPacketOverhead = 6; -static const unsigned int kPacketDataMaxSize = 100; -static const uint8_t kPacketStartString[] = {0x55, 0x55}; - -static uint16_t crc16(const uint8_t *data, int count); -static uint16_t crc_update(uint16_t crc, uint8_t data); - -void encode_serial_msg(const kr_mav_msgs::Serial &msg, std::vector &serial_data) -{ - const uint8_t data_length = msg.data.size(); - - if(data_length > kPacketDataMaxSize) - { - std::cerr << "encode_serial_msg: Message too large: " << msg.data.size() << ", max: " << kPacketDataMaxSize - << std::endl; - return; - } - - serial_data.resize(kPacketOverhead + data_length); - serial_data[0] = kPacketStartString[0]; - serial_data[1] = kPacketStartString[1]; - serial_data[2] = data_length; - serial_data[3] = msg.type; - - uint16_t crc = crc16(&data_length, 1); - crc = crc_update(crc, msg.type); - - memcpy(&(serial_data[4]), &(msg.data[0]), data_length); - for(int i = 0; i < data_length; i++) - crc = crc_update(crc, msg.data[i]); - - serial_data[4 + data_length + 0] = crc & 0xFF; - serial_data[4 + data_length + 1] = crc >> 8; -} - -void process_serial_data(const uint8_t *data, const size_t count, boost::function callback) -{ - static kr_mav_msgs::Serial serial_msg; - static enum PacketState state = kPacketStart1; - static uint16_t received_length = 0, data_count = 0; - ; - static uint16_t expected_crc = 0, received_crc = 0; - - for(size_t i = 0; i < count; i++) - { - const uint8_t c = data[i]; - // printf("%d, %X\n", state, c); - if(state == kPacketStart1 && c == kPacketStartString[0]) - state = kPacketStart2; - else if(state == kPacketStart2 && c == kPacketStartString[1]) - state = kPacketCount; - else if(state == kPacketCount) - { - received_length = c; - if(received_length > kPacketDataMaxSize) - state = kPacketStart1; - else - { - expected_crc = crc16(&c, 1); - serial_msg.data.resize(received_length); - data_count = 0; - state = kPacketType; - } - } - else if(state == kPacketType) - { - serial_msg.type = c; - expected_crc = crc_update(expected_crc, c); - if(received_length > 0) - state = kPacketData; - else - state = kPacketCRCL; - } - else if(state == kPacketData) - { - serial_msg.data[data_count] = c; - expected_crc = crc_update(expected_crc, c); - data_count++; - if(data_count == received_length) - state = kPacketCRCL; - } - else if(state == kPacketCRCL) - { - received_crc = c; - state = kPacketCRCH; - } - else if(state == kPacketCRCH) - { - received_crc = received_crc + 256 * c; - if(expected_crc == received_crc) - { - // Received complete packet - callback(serial_msg); - } - state = kPacketStart1; - } - else - state = kPacketStart1; - } -} - -// CRC-16-ITU-T -static uint16_t crc_update(uint16_t crc, uint8_t data) -{ - uint16_t x = ((crc >> 8) ^ data); - x ^= x >> 4; - - crc = (crc << 8) ^ (x << 12) ^ (x << 5) ^ x; - - return crc; -} - -static uint16_t crc16(const uint8_t *data, int count) -{ - uint16_t crc = 0xffff; - - for(uint16_t i = 0; i < count; i++) - { - crc = crc_update(crc, data[i]); - } - return crc; -} diff --git a/interfaces/kr_serial_interface/src/decode_msgs.cpp b/interfaces/kr_serial_interface/src/decode_msgs.cpp deleted file mode 100644 index 8848ae3e..00000000 --- a/interfaces/kr_serial_interface/src/decode_msgs.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include -#include - -#include - -namespace kr_mav_msgs -{ -bool decodeOutputData(const std::vector &data, kr_mav_msgs::OutputData &output) -{ - struct OUTPUT_DATA output_data; - if(data.size() != sizeof(output_data)) - return false; - - memcpy(&output_data, &data[0], sizeof(output_data)); - output.loop_rate = output_data.loop_rate; - output.voltage = output_data.voltage / 1e3f; - - const double roll = output_data.roll / 1e2f * M_PI / 180; - const double pitch = output_data.pitch / 1e2f * M_PI / 180; - const double yaw = output_data.yaw / 1e2f * M_PI / 180; - // Asctec (2012 firmware) uses Z-Y-X convention - Eigen::Quaternionf q = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) * - Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) * - Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()); - output.orientation.w = q.w(); - output.orientation.x = q.x(); - output.orientation.y = q.y(); - output.orientation.z = q.z(); - - output.angular_velocity.x = output_data.ang_vel[0] * 0.0154f * M_PI / 180; - output.angular_velocity.y = output_data.ang_vel[1] * 0.0154f * M_PI / 180; - output.angular_velocity.z = output_data.ang_vel[2] * 0.0154f * M_PI / 180; - - output.linear_acceleration.x = output_data.acc[0] / 1e3f * 9.81f; - output.linear_acceleration.y = output_data.acc[1] / 1e3f * 9.81f; - output.linear_acceleration.z = output_data.acc[2] / 1e3f * 9.81f; - - output.pressure_dheight = output_data.dheight / 1e3f; - output.pressure_height = output_data.height / 1e3f; - - output.magnetic_field.x = output_data.mag[0] / 2500.0f; - output.magnetic_field.y = output_data.mag[1] / 2500.0f; - output.magnetic_field.z = output_data.mag[2] / 2500.0f; - - for(int i = 0; i < 8; i++) - { - output.radio_channel[i] = output_data.radio[i]; - } - - // Asctec firmware uses the following rotor numbering convention: - // *1* Front - // 3 4 - // 2 - // - // But we want: - // *1* Front - // 2 4 - // 3 - const int motors_map[] = {0, 2, 1, 3}; - for(int i = 0; i < 4; i++) - { - // The following conversion is from - // http://wiki.asctec.de/display/AR/List+of+all+predefined+variables%2C+commands+and+parameters - // - // motorRPM = 1075+m*37.625 - // - // Note: If m == 0, the motors are not spinning. - int m = output_data.rpm[motors_map[i]]; - output.motor_rpm[i] = (m == 0 ? 0 : 1075) + m * 37.625; - } - - output.seq = output_data.seq; - - return true; -} - -bool decodeStatusData(const std::vector &data, kr_mav_msgs::StatusData &status) -{ - struct STATUS_DATA status_data; - if(data.size() != sizeof(status_data)) - return false; - memcpy(&status_data, &data[0], sizeof(status_data)); - - status.loop_rate = status_data.loop_rate; - status.voltage = status_data.voltage / 1e3f; - status.seq = status_data.seq; - - return true; -} - -} // namespace kr_mav_msgs diff --git a/interfaces/kr_serial_interface/src/encode_msgs.cpp b/interfaces/kr_serial_interface/src/encode_msgs.cpp deleted file mode 100644 index 2d0dabf5..00000000 --- a/interfaces/kr_serial_interface/src/encode_msgs.cpp +++ /dev/null @@ -1,105 +0,0 @@ -#include -#include -#include - -#include -#include - -namespace kr_mav_msgs -{ -namespace -{ -template -using removeref = typename std::remove_reference::type; - -template -removeref saturation_cast(Source const src, char const *variable_name = "") -{ - try - { - return boost::numeric_cast>(src); - } - catch(const boost::numeric::negative_overflow &e) - { - ROS_WARN("Negative overflow when setting %s", variable_name); - return std::numeric_limits>::lowest(); - } - catch(const boost::numeric::positive_overflow &e) - { - ROS_WARN("Positive overflow when setting %s", variable_name); - return std::numeric_limits>::max(); - } -} -} // namespace - -// NOTE(Kartik): Macro needed in order to get the tgt variable name as a string -#define SATURATE_CAST(tgt, src) tgt = saturation_cast(src, #tgt) - -void encodeSO3Command(const kr_mav_msgs::SO3Command &so3_command, std::vector &output) -{ - struct SO3_CMD_INPUT so3_cmd_input; - - SATURATE_CAST(so3_cmd_input.force[0], so3_command.force.x * 500); - SATURATE_CAST(so3_cmd_input.force[1], so3_command.force.y * 500); - SATURATE_CAST(so3_cmd_input.force[2], so3_command.force.z * 500); - - SATURATE_CAST(so3_cmd_input.des_qx, so3_command.orientation.x * 125); - SATURATE_CAST(so3_cmd_input.des_qy, so3_command.orientation.y * 125); - SATURATE_CAST(so3_cmd_input.des_qz, so3_command.orientation.z * 125); - SATURATE_CAST(so3_cmd_input.des_qw, so3_command.orientation.w * 125); - - SATURATE_CAST(so3_cmd_input.angvel_x, so3_command.angular_velocity.x * 1000); - SATURATE_CAST(so3_cmd_input.angvel_y, so3_command.angular_velocity.y * 1000); - SATURATE_CAST(so3_cmd_input.angvel_z, so3_command.angular_velocity.z * 1000); - - SATURATE_CAST(so3_cmd_input.kR[0], so3_command.kR[0] * 50); - SATURATE_CAST(so3_cmd_input.kR[1], so3_command.kR[1] * 50); - SATURATE_CAST(so3_cmd_input.kR[2], so3_command.kR[2] * 50); - - SATURATE_CAST(so3_cmd_input.kOm[0], so3_command.kOm[0] * 100); - SATURATE_CAST(so3_cmd_input.kOm[1], so3_command.kOm[1] * 100); - SATURATE_CAST(so3_cmd_input.kOm[2], so3_command.kOm[2] * 100); - - SATURATE_CAST(so3_cmd_input.cur_yaw, so3_command.aux.current_yaw * 1e4f); - - SATURATE_CAST(so3_cmd_input.kf_correction, so3_command.aux.kf_correction * 1e11f); - SATURATE_CAST(so3_cmd_input.angle_corrections[0], so3_command.aux.angle_corrections[0] * 2500); - SATURATE_CAST(so3_cmd_input.angle_corrections[1], so3_command.aux.angle_corrections[1] * 2500); - - so3_cmd_input.enable_motors = so3_command.aux.enable_motors; - so3_cmd_input.use_external_yaw = so3_command.aux.use_external_yaw; - - so3_cmd_input.seq = so3_command.header.seq % 255; - - output.resize(sizeof(so3_cmd_input)); - memcpy(&output[0], &so3_cmd_input, sizeof(so3_cmd_input)); -} - -void encodeTRPYCommand(const kr_mav_msgs::TRPYCommand &trpy_command, std::vector &output) -{ - struct TRPY_CMD trpy_cmd_input; - - SATURATE_CAST(trpy_cmd_input.thrust, trpy_command.thrust * 1e4f); - SATURATE_CAST(trpy_cmd_input.roll, trpy_command.roll * 1e4f); - SATURATE_CAST(trpy_cmd_input.pitch, trpy_command.pitch * 1e4f); - SATURATE_CAST(trpy_cmd_input.yaw, trpy_command.yaw * 1e4f); - SATURATE_CAST(trpy_cmd_input.current_yaw, trpy_command.aux.current_yaw * 1e4f); - - trpy_cmd_input.enable_motors = trpy_command.aux.enable_motors; - trpy_cmd_input.use_external_yaw = trpy_command.aux.use_external_yaw; - - output.resize(sizeof(trpy_cmd_input)); - memcpy(&output[0], &trpy_cmd_input, sizeof(trpy_cmd_input)); -} - -void encodePWMCommand(const kr_mav_msgs::PWMCommand &pwm_command, std::vector &output) -{ - struct PWM_CMD_INPUT pwm_cmd_input; - - SATURATE_CAST(pwm_cmd_input.pwm[0], pwm_command.pwm[0] * 255); - SATURATE_CAST(pwm_cmd_input.pwm[1], pwm_command.pwm[1] * 255); - - output.resize(sizeof(pwm_cmd_input)); - memcpy(&output[0], &pwm_cmd_input, sizeof(pwm_cmd_input)); -} -} // namespace kr_mav_msgs diff --git a/interfaces/kr_serial_interface/src/quad_decode_msg_nodelet.cpp b/interfaces/kr_serial_interface/src/quad_decode_msg_nodelet.cpp deleted file mode 100644 index 999095d1..00000000 --- a/interfaces/kr_serial_interface/src/quad_decode_msg_nodelet.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include -#include -#include -#include -#include - -class QuadDecodeMsg : public nodelet::Nodelet -{ - public: - void onInit(void); - - private: - void serial_callback(const kr_mav_msgs::Serial::ConstPtr &msg); - ros::Publisher output_data_pub_, imu_output_pub_, status_pub_; - ros::Subscriber serial_sub_; -}; - -void QuadDecodeMsg::serial_callback(const kr_mav_msgs::Serial::ConstPtr &msg) -{ - if(msg->type == kr_mav_msgs::Serial::OUTPUT_DATA) - { - kr_mav_msgs::OutputData::Ptr output_msg(new kr_mav_msgs::OutputData); - sensor_msgs::Imu::Ptr imu_msg(new sensor_msgs::Imu); - - if(kr_mav_msgs::decodeOutputData(msg->data, *output_msg)) - { - output_msg->header.stamp = msg->header.stamp; - output_msg->header.frame_id = "/quadrotor"; - output_data_pub_.publish(output_msg); - - imu_msg->header = output_msg->header; - imu_msg->orientation = output_msg->orientation; - imu_msg->angular_velocity = output_msg->angular_velocity; - imu_msg->linear_acceleration = output_msg->linear_acceleration; - imu_output_pub_.publish(imu_msg); - } - } - else if(msg->type == kr_mav_msgs::Serial::STATUS_DATA) - { - kr_mav_msgs::StatusData::Ptr status_msg(new kr_mav_msgs::StatusData); - if(kr_mav_msgs::decodeStatusData(msg->data, *status_msg)) - { - status_msg->header.stamp = msg->header.stamp; - status_msg->header.frame_id = "/quadrotor"; - status_pub_.publish(status_msg); - } - } -} - -void QuadDecodeMsg::onInit(void) -{ - ros::NodeHandle priv_nh(getPrivateNodeHandle()); - - output_data_pub_ = priv_nh.advertise("output_data", 10); - imu_output_pub_ = priv_nh.advertise("imu", 10); - status_pub_ = priv_nh.advertise("status", 10); - - serial_sub_ = - priv_nh.subscribe("serial", 10, &QuadDecodeMsg::serial_callback, this, ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(QuadDecodeMsg, nodelet::Nodelet); diff --git a/interfaces/kr_serial_interface/src/quad_encode_msg.cpp b/interfaces/kr_serial_interface/src/quad_encode_msg.cpp deleted file mode 100644 index a6e76264..00000000 --- a/interfaces/kr_serial_interface/src/quad_encode_msg.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#include -#include -#include -#include -#include -#include - -static ros::Publisher serial_msg_pub; -static int channel; - -static void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg) -{ - kr_mav_msgs::Serial serial_msg; - serial_msg.header.seq = msg->header.seq; - serial_msg.channel = channel; - serial_msg.type = kr_mav_msgs::Serial::SO3_CMD; - - kr_mav_msgs::encodeSO3Command(*msg, serial_msg.data); - - serial_msg.header.stamp = ros::Time::now(); - serial_msg_pub.publish(serial_msg); -} - -static void trpy_cmd_callback(const kr_mav_msgs::TRPYCommand::ConstPtr &msg) -{ - kr_mav_msgs::Serial serial_msg; - serial_msg.header.seq = msg->header.seq; - serial_msg.channel = channel; - serial_msg.type = kr_mav_msgs::Serial::TRPY_CMD; - - kr_mav_msgs::encodeTRPYCommand(*msg, serial_msg.data); - - serial_msg.header.stamp = ros::Time::now(); - serial_msg_pub.publish(serial_msg); -} - -static void pwm_cmd_callback(const kr_mav_msgs::PWMCommand::ConstPtr &msg) -{ - kr_mav_msgs::Serial serial_msg; - serial_msg.header.seq = msg->header.seq; - serial_msg.channel = channel; - serial_msg.type = kr_mav_msgs::Serial::PWM_CMD; - - kr_mav_msgs::encodePWMCommand(*msg, serial_msg.data); - - serial_msg.header.stamp = ros::Time::now(); - serial_msg_pub.publish(serial_msg); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "quad_encode_msg"); - - ros::NodeHandle n("~"); - - n.param("channel", channel, 0); - - ros::Subscriber so3_cmd_sub = n.subscribe("so3_cmd", 10, &so3_cmd_callback, ros::TransportHints().tcpNoDelay()); - - ros::Subscriber trpy_cmd_sub = n.subscribe("trpy_cmd", 10, &so3_cmd_callback, ros::TransportHints().tcpNoDelay()); - - ros::Subscriber pwm_cmd_sub = n.subscribe("pwm_cmd", 10, &pwm_cmd_callback, ros::TransportHints().tcpNoDelay()); - - serial_msg_pub = n.advertise("serial_msg", 10); - - ros::spin(); - - return 0; -} diff --git a/interfaces/kr_serial_interface/src/quad_encode_msg_nodelet.cpp b/interfaces/kr_serial_interface/src/quad_encode_msg_nodelet.cpp deleted file mode 100644 index efcaae2f..00000000 --- a/interfaces/kr_serial_interface/src/quad_encode_msg_nodelet.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -class QuadEncodeMsg : public nodelet::Nodelet -{ - public: - void onInit(void); - - private: - void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg); - void trpy_cmd_callback(const kr_mav_msgs::TRPYCommand::ConstPtr &msg); - void pwm_cmd_callback(const kr_mav_msgs::PWMCommand::ConstPtr &msg); - ros::Publisher serial_msg_pub_; - ros::Subscriber so3_cmd_sub_; - ros::Subscriber trpy_cmd_sub_; - ros::Subscriber pwm_cmd_sub_; - int channel_; -}; - -void QuadEncodeMsg::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg) -{ - kr_mav_msgs::Serial::Ptr serial_msg(new kr_mav_msgs::Serial); - serial_msg->header.seq = msg->header.seq; - serial_msg->channel = channel_; - serial_msg->type = kr_mav_msgs::Serial::SO3_CMD; - - kr_mav_msgs::encodeSO3Command(*msg, serial_msg->data); - - serial_msg->header.stamp = ros::Time::now(); - serial_msg_pub_.publish(serial_msg); -} - -void QuadEncodeMsg::trpy_cmd_callback(const kr_mav_msgs::TRPYCommand::ConstPtr &msg) -{ - kr_mav_msgs::Serial::Ptr serial_msg(new kr_mav_msgs::Serial); - serial_msg->header.seq = msg->header.seq; - serial_msg->channel = channel_; - serial_msg->type = kr_mav_msgs::Serial::TRPY_CMD; - - kr_mav_msgs::encodeTRPYCommand(*msg, serial_msg->data); - - serial_msg->header.stamp = ros::Time::now(); - serial_msg_pub_.publish(serial_msg); -} - -void QuadEncodeMsg::pwm_cmd_callback(const kr_mav_msgs::PWMCommand::ConstPtr &msg) -{ - kr_mav_msgs::Serial::Ptr serial_msg(new kr_mav_msgs::Serial); - serial_msg->header.seq = msg->header.seq; - serial_msg->channel = channel_; - serial_msg->type = kr_mav_msgs::Serial::PWM_CMD; - kr_mav_msgs::encodePWMCommand(*msg, serial_msg->data); - - serial_msg->header.stamp = ros::Time::now(); - serial_msg_pub_.publish(serial_msg); -} - -void QuadEncodeMsg::onInit(void) -{ - ros::NodeHandle priv_nh(getPrivateNodeHandle()); - - priv_nh.param("channel", channel_, 0); - - serial_msg_pub_ = priv_nh.advertise("serial_msg", 10); - - so3_cmd_sub_ = - priv_nh.subscribe("so3_cmd", 10, &QuadEncodeMsg::so3_cmd_callback, this, ros::TransportHints().tcpNoDelay()); - trpy_cmd_sub_ = - priv_nh.subscribe("trpy_cmd", 10, &QuadEncodeMsg::trpy_cmd_callback, this, ros::TransportHints().tcpNoDelay()); - pwm_cmd_sub_ = - priv_nh.subscribe("pwm_cmd", 10, &QuadEncodeMsg::pwm_cmd_callback, this, ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(QuadEncodeMsg, nodelet::Nodelet); diff --git a/interfaces/kr_serial_interface/src/quad_serial_comm_nodelet.cpp b/interfaces/kr_serial_interface/src/quad_serial_comm_nodelet.cpp deleted file mode 100644 index 7efacc7c..00000000 --- a/interfaces/kr_serial_interface/src/quad_serial_comm_nodelet.cpp +++ /dev/null @@ -1,70 +0,0 @@ -#include -#include -#include -#include -#include - -class QuadSerialComm : public nodelet::Nodelet -{ - public: - void onInit(void); - ~QuadSerialComm(); - - private: - void serial_callback(const kr_mav_msgs::Serial::ConstPtr &msg); - void output_data_callback(kr_mav_msgs::Serial &msg); - void serial_read_callback(const unsigned char *data, size_t count); - - ASIOSerialDevice sd_; - ros::Publisher output_data_pub_; - ros::Subscriber serial_sub_; -}; - -void QuadSerialComm::serial_callback(const kr_mav_msgs::Serial::ConstPtr &msg) -{ - std::vector serial_msg; - - encode_serial_msg(*msg, serial_msg); - - sd_.Write(serial_msg); -} - -void QuadSerialComm::output_data_callback(kr_mav_msgs::Serial &msg) -{ - msg.header.stamp = ros::Time::now(); - output_data_pub_.publish(msg); -} - -void QuadSerialComm::serial_read_callback(const unsigned char *data, size_t count) -{ - process_serial_data(data, count, boost::bind(&QuadSerialComm::output_data_callback, this, _1)); -} - -void QuadSerialComm::onInit(void) -{ - ros::NodeHandle n(getPrivateNodeHandle()); - - std::string device; - n.param("device", device, std::string("/dev/ttyUSB0")); - - int baud_rate; - n.param("baud_rate", baud_rate, 57600); - - sd_.Open(device, baud_rate); - - output_data_pub_ = n.advertise("from_robot", 10); - - serial_sub_ = n.subscribe("to_robot", 10, &QuadSerialComm::serial_callback, this, ros::TransportHints().tcpNoDelay()); - - sd_.SetReadCallback(boost::bind(&QuadSerialComm::serial_read_callback, this, _1, _2)); - sd_.Start(); -} - -QuadSerialComm::~QuadSerialComm() -{ - sd_.Close(); - sd_.Stop(); -} - -#include -PLUGINLIB_EXPORT_CLASS(QuadSerialComm, nodelet::Nodelet); diff --git a/kr_quadrotor_simulator/CMakeLists.txt b/kr_quadrotor_simulator/CMakeLists.txt deleted file mode 100644 index 8d08de86..00000000 --- a/kr_quadrotor_simulator/CMakeLists.txt +++ /dev/null @@ -1,53 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(kr_quadrotor_simulator) - -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) -endif() - -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) - -find_package(catkin REQUIRED COMPONENTS geometry_msgs kr_mav_msgs nav_msgs roscpp sensor_msgs tf2_ros) -find_package(Eigen3 REQUIRED) - -catkin_package( - CATKIN_DEPENDS - geometry_msgs - kr_mav_msgs - nav_msgs - roscpp - sensor_msgs - tf2_ros - DEPENDS - EIGEN3 -) - -add_library(${PROJECT_NAME} INTERFACE) -target_include_directories(${PROJECT_NAME} INTERFACE ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} INTERFACE ${catkin_LIBRARIES}) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) - -add_library(kr_quadrotor_dynamics src/dynamics/Quadrotor.cpp) -target_include_directories(kr_quadrotor_dynamics PUBLIC include) -target_link_libraries(kr_quadrotor_dynamics PUBLIC Eigen3::Eigen ${PROJECT_NAME}) - -add_executable(${PROJECT_NAME}_so3 src/quadrotor_simulator_so3.cpp) -target_link_libraries(${PROJECT_NAME}_so3 PUBLIC kr_quadrotor_dynamics) -# add_dependencies(${PROJECT_NAME}_so3 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) - -add_executable(${PROJECT_NAME}_trpy src/quadrotor_simulator_trpy.cpp) -target_link_libraries(${PROJECT_NAME}_trpy PUBLIC kr_quadrotor_dynamics) -# add_dependencies(${PROJECT_NAME}_trpy ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) - -install( - TARGETS kr_quadrotor_dynamics ${PROJECT_NAME}_so3 ${PROJECT_NAME}_trpy - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) diff --git a/kr_quadrotor_simulator/config/crazyflie_params.yaml b/kr_quadrotor_simulator/config/crazyflie_params.yaml deleted file mode 100644 index fa106d40..00000000 --- a/kr_quadrotor_simulator/config/crazyflie_params.yaml +++ /dev/null @@ -1,13 +0,0 @@ -gravity: 9.81 -mass: 0.029 -prop_radius: 0.0225 -arm_length: 0.046 -motor_time_constant: 0.00001 # TODO -min_rpm: 0 # TODO -max_rpm: 10000 #TODO -# The following values are from: -# http://groups.csail.mit.edu/robotics-center/public_papers/Landry15.pdf -Ixx: 2.395e-5 -Iyy: 2.395e-5 -Izz: 3.235e-5 -thrust_coefficient: 5.507e-5 # (0.005022 N s^2 / rad^2) * (1 min / 60 s)^2 * (2*pi rad / rev)^2 diff --git a/kr_quadrotor_simulator/config/hummingbird_params.yaml b/kr_quadrotor_simulator/config/hummingbird_params.yaml deleted file mode 100644 index 936a9dd0..00000000 --- a/kr_quadrotor_simulator/config/hummingbird_params.yaml +++ /dev/null @@ -1,12 +0,0 @@ -gravity: 9.81 -mass: 0.5 -Ixx: 2.64e-3 -Iyy: 2.64e-3 -Izz: 4.96e-3 -thrust_coefficient: 5.55e-8 -prop_radius: 0.099 -arm_length: 0.17 -motor_time_constant: 0.05 -min_rpm: 1500 -max_rpm: 7500 -drag_coefficient: 0.0 diff --git a/kr_quadrotor_simulator/config/qualcomm_quad_params.yaml b/kr_quadrotor_simulator/config/qualcomm_quad_params.yaml deleted file mode 100644 index d51a3f62..00000000 --- a/kr_quadrotor_simulator/config/qualcomm_quad_params.yaml +++ /dev/null @@ -1,12 +0,0 @@ -gravity: 9.81 -mass: 0.25 -Ixx: 0.000601 -Iyy: 0.000589 -Izz: 0.001076 -thrust_coefficient: 4.1790e-09 -prop_radius: 0.05 -arm_length: 0.1075 -motor_time_constant: 0.05 -min_rpm: 5500 -max_rpm: 16400 -drag_coefficient: 0.0 \ No newline at end of file diff --git a/kr_quadrotor_simulator/include/kr_quadrotor_simulator/Quadrotor.h b/kr_quadrotor_simulator/include/kr_quadrotor_simulator/Quadrotor.h deleted file mode 100644 index fd1079c4..00000000 --- a/kr_quadrotor_simulator/include/kr_quadrotor_simulator/Quadrotor.h +++ /dev/null @@ -1,104 +0,0 @@ -#ifndef QUADROTOR_SIMULATOR_QUADROTOR_H -#define QUADROTOR_SIMULATOR_QUADROTOR_H - -#include -#include - -namespace QuadrotorSimulator -{ -class Quadrotor -{ - public: - struct State - { - Eigen::Vector3d x; - Eigen::Vector3d v; - Eigen::Matrix3d R; - Eigen::Vector3d omega; - Eigen::Array4d motor_rpm; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; - }; - - Quadrotor(); - - const Quadrotor::State &getState() const; - void setState(const Quadrotor::State &state); - - double getMass() const; - void setMass(double mass); - - double getDragCoefficient() const; - void setDragCoefficient(double drag_coefficient); - - double getGravity() const; - void setGravity(double g); - - const Eigen::Matrix3d &getInertia() const; - void setInertia(const Eigen::Matrix3d &inertia); - - double getArmLength() const; - void setArmLength(double d); - - double getPropRadius() const; - void setPropRadius(double r); - - double getPropellerThrustCoefficient() const; - void setPropellerThrustCoefficient(double kf); - - double getPropellerMomentCoefficient() const; - void setPropellerMomentCoefficient(double km); - - double getMotorTimeConstant() const; - void setMotorTimeConstant(double k); - - const Eigen::Vector3d &getExternalForce() const; - void setExternalForce(const Eigen::Vector3d &force); - - const Eigen::Vector3d &getExternalMoment() const; - void setExternalMoment(const Eigen::Vector3d &moment); - - double getMaxRPM() const; - void setMaxRPM(double max_rpm); - - double getMinRPM() const; - void setMinRPM(double min_rpm); - - // Inputs are desired RPM for the motors - // Rotor numbering is: - // *1* Front - // 3 4 - // 2 - // with 1 and 2 clockwise and 3 and 4 counter-clockwise (looking from top) - void setInput(double u1, double u2, double u3, double u4); - - // Runs the actual dynamics simulation with a time step of dt - void step(double dt); - - // For internal use, but needs to be public for odeint - typedef boost::array InternalState; - void operator()(const Quadrotor::InternalState &x, Quadrotor::InternalState &dxdt, const double /* t */); - - private: - void updateInternalState(); - - double g_; // gravity - double mass_; - Eigen::Matrix3d J_; // Inertia - double kf_; - double km_; - double prop_radius_; - double arm_length_; - double motor_time_constant_; // unit: sec - double max_rpm_; - double min_rpm_; - double drag_coefficient_; - Quadrotor::State state_; - Eigen::Array4d input_; - Eigen::Vector3d external_force_; - Eigen::Vector3d external_moment_; - - InternalState internal_state_; -}; - -} // namespace QuadrotorSimulator -#endif diff --git a/kr_quadrotor_simulator/package.xml b/kr_quadrotor_simulator/package.xml deleted file mode 100644 index 5d359e1a..00000000 --- a/kr_quadrotor_simulator/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - kr_quadrotor_simulator - 1.0.0 - kr_quadrotor_simulator - Kartik Mohta - Michael Watterson - - BSD - - Kartik Mohta - - catkin - - geometry_msgs - kr_mav_msgs - nav_msgs - roscpp - sensor_msgs - tf2_ros - diff --git a/kr_quadrotor_simulator/src/dynamics/Quadrotor.cpp b/kr_quadrotor_simulator/src/dynamics/Quadrotor.cpp deleted file mode 100644 index b0f4b853..00000000 --- a/kr_quadrotor_simulator/src/dynamics/Quadrotor.cpp +++ /dev/null @@ -1,370 +0,0 @@ -#include "kr_quadrotor_simulator/Quadrotor.h" - -#include -#include -#include -#include - -namespace odeint = boost::numeric::odeint; - -namespace QuadrotorSimulator -{ -Quadrotor::Quadrotor() -{ - g_ = 9.81; - mass_ = 0.5; - double Ixx = 2.64e-3, Iyy = 2.64e-3, Izz = 4.96e-3; - prop_radius_ = 0.099; - J_ = Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal(); - - kf_ = 5.55e-8; - // km_ = 2.5e-9; // from Nate - // km = (Cq/Ct)*Dia*kf - // Cq/Ct for 8 inch props from UIUC prop db ~ 0.07 - km_ = 0.07 * (2 * prop_radius_) * kf_; - - arm_length_ = 0.17; - motor_time_constant_ = 1.0 / 20; - min_rpm_ = 1500; - max_rpm_ = 7500; - - state_.x = Eigen::Vector3d::Zero(); - state_.v = Eigen::Vector3d::Zero(); - state_.R = Eigen::Matrix3d::Identity(); - state_.omega = Eigen::Vector3d::Zero(); - state_.motor_rpm = Eigen::Array4d::Zero(); - updateInternalState(); - - input_ = Eigen::Array4d::Zero(); - external_force_ = Eigen::Vector3d::Zero(); - external_moment_ = Eigen::Vector3d::Zero(); -} - -void Quadrotor::step(double dt) -{ - odeint::integrate(boost::ref(*this), internal_state_, 0.0, dt, dt); - - for(int i = 0; i < 3; i++) - { - state_.x(i) = internal_state_[0 + i]; - state_.v(i) = internal_state_[3 + i]; - state_.R(i, 0) = internal_state_[6 + i]; - state_.R(i, 1) = internal_state_[9 + i]; - state_.R(i, 2) = internal_state_[12 + i]; - state_.omega(i) = internal_state_[15 + i]; - } - state_.motor_rpm(0) = internal_state_[18]; - state_.motor_rpm(1) = internal_state_[19]; - state_.motor_rpm(2) = internal_state_[20]; - state_.motor_rpm(3) = internal_state_[21]; - - // Re-orthonormalize R (polar decomposition) - Eigen::LLT llt(state_.R.transpose() * state_.R); - Eigen::Matrix3d P = llt.matrixL(); - Eigen::Matrix3d R = state_.R * P.inverse(); - state_.R = R; - - // Don't go below zero, simulate floor - if(state_.x(2) < 0.0 && state_.v(2) < 0) - { - state_.x(2) = 0; - state_.v(2) = 0; - } - updateInternalState(); -} - -void Quadrotor::operator()(const Quadrotor::InternalState &x, Quadrotor::InternalState &dxdt, const double /* t */) -{ - State cur_state; - for(int i = 0; i < 3; i++) - { - cur_state.x(i) = x[0 + i]; - cur_state.v(i) = x[3 + i]; - cur_state.R(i, 0) = x[6 + i]; - cur_state.R(i, 1) = x[9 + i]; - cur_state.R(i, 2) = x[12 + i]; - cur_state.omega(i) = x[15 + i]; - } - for(int i = 0; i < 4; i++) - { - cur_state.motor_rpm(i) = x[18 + i]; - } - - // Re-orthonormalize R (polar decomposition) - Eigen::LLT llt(cur_state.R.transpose() * cur_state.R); - Eigen::Matrix3d P = llt.matrixL(); - Eigen::Matrix3d R = cur_state.R * P.inverse(); - - Eigen::Vector3d x_dot, v_dot, omega_dot; - Eigen::Matrix3d R_dot; - Eigen::Array4d motor_rpm_dot; - Eigen::Array4d motor_rpm_sq; - Eigen::Matrix3d omega_hat(Eigen::Matrix3d::Zero()); - - omega_hat(2, 1) = cur_state.omega(0); - omega_hat(1, 2) = -cur_state.omega(0); - omega_hat(0, 2) = cur_state.omega(1); - omega_hat(2, 0) = -cur_state.omega(1); - omega_hat(1, 0) = cur_state.omega(2); - omega_hat(0, 1) = -cur_state.omega(2); - - if(motor_time_constant_ == 0) - { - motor_rpm_dot = Eigen::Array4d::Zero(); - // NOTE(Kartik): Directly modifying internal_state here - internal_state_[18] = input_(0); - internal_state_[19] = input_(1); - internal_state_[20] = input_(2); - internal_state_[21] = input_(3); - cur_state.motor_rpm = input_; - } - else - motor_rpm_dot = (input_ - cur_state.motor_rpm) / motor_time_constant_; - - motor_rpm_sq = cur_state.motor_rpm.square(); - - double thrust = kf_ * motor_rpm_sq.sum(); - Eigen::Vector3d moments; - moments(0) = kf_ * (motor_rpm_sq(2) - motor_rpm_sq(3)) * arm_length_; - moments(1) = kf_ * (motor_rpm_sq(1) - motor_rpm_sq(0)) * arm_length_; - moments(2) = km_ * (motor_rpm_sq(0) + motor_rpm_sq(1) - motor_rpm_sq(2) - motor_rpm_sq(3)); - - x_dot = cur_state.v; - v_dot = -Eigen::Vector3d(0, 0, g_) + thrust * R.col(2) / mass_ + external_force_ / mass_; - if(drag_coefficient_ != 0) - { - Eigen::Matrix3d P; - P << 1, 0, 0, 0, 1, 0, 0, 0, 0; - v_dot -= drag_coefficient_ / mass_ * R * P * R.transpose() * cur_state.v; - } - R_dot = R * omega_hat; - omega_dot = J_.inverse() * (moments - cur_state.omega.cross(J_ * cur_state.omega) + external_moment_); - - for(int i = 0; i < 3; i++) - { - dxdt[0 + i] = x_dot(i); - dxdt[3 + i] = v_dot(i); - dxdt[6 + i] = R_dot(i, 0); - dxdt[9 + i] = R_dot(i, 1); - dxdt[12 + i] = R_dot(i, 2); - dxdt[15 + i] = omega_dot(i); - } - for(int i = 0; i < 4; i++) - { - dxdt[18 + i] = motor_rpm_dot(i); - } -} - -void Quadrotor::setInput(double u1, double u2, double u3, double u4) -{ - input_(0) = u1; - input_(1) = u2; - input_(2) = u3; - input_(3) = u4; - if(u1 != 0 || u2 != 0 || u3 != 0 || u4 != 0) // Limit to min/max RPM if any of the RPMs are non-zero - { - for(int i = 0; i < 4; i++) - { - if(input_(i) > max_rpm_) - input_(i) = max_rpm_; - else if(input_(i) < min_rpm_) - input_(i) = min_rpm_; - } - } -} - -const Quadrotor::State &Quadrotor::getState() const -{ - return state_; -} -void Quadrotor::setState(const Quadrotor::State &state) -{ - state_.x = state.x; - state_.v = state.v; - state_.R = state.R; - state_.omega = state.omega; - state_.motor_rpm = state.motor_rpm; - - updateInternalState(); -} - -double Quadrotor::getMass() const -{ - return mass_; -} -void Quadrotor::setMass(double mass) -{ - mass_ = mass; -} - -double Quadrotor::getDragCoefficient() const -{ - return drag_coefficient_; -} - -void Quadrotor::setDragCoefficient(double drag_coefficient) -{ - drag_coefficient_ = drag_coefficient; -} - -double Quadrotor::getGravity() const -{ - return g_; -} -void Quadrotor::setGravity(double g) -{ - g_ = g; -} - -const Eigen::Matrix3d &Quadrotor::getInertia() const -{ - return J_; -} -void Quadrotor::setInertia(const Eigen::Matrix3d &inertia) -{ - if(inertia != inertia.transpose()) - { - std::cerr << "Inertia matrix not symmetric, not setting" << std::endl; - return; - } - J_ = inertia; -} - -double Quadrotor::getArmLength() const -{ - return arm_length_; -} -void Quadrotor::setArmLength(double d) -{ - if(d <= 0) - { - std::cerr << "Arm length <= 0, not setting" << std::endl; - return; - } - - arm_length_ = d; -} - -double Quadrotor::getPropRadius() const -{ - return prop_radius_; -} -void Quadrotor::setPropRadius(double r) -{ - if(r <= 0) - { - std::cerr << "Prop radius <= 0, not setting" << std::endl; - return; - } - prop_radius_ = r; -} - -double Quadrotor::getPropellerThrustCoefficient() const -{ - return kf_; -} -void Quadrotor::setPropellerThrustCoefficient(double kf) -{ - if(kf <= 0) - { - std::cerr << "Thrust coefficient <= 0, not setting" << std::endl; - return; - } - - kf_ = kf; -} - -double Quadrotor::getPropellerMomentCoefficient() const -{ - return km_; -} -void Quadrotor::setPropellerMomentCoefficient(double km) -{ - if(km <= 0) - { - std::cerr << "Moment coefficient <= 0, not setting" << std::endl; - return; - } - - km_ = km; -} - -double Quadrotor::getMotorTimeConstant() const -{ - return motor_time_constant_; -} -void Quadrotor::setMotorTimeConstant(double k) -{ - if(k < 0) - { - std::cerr << "Motor time constant <= 0, not setting" << std::endl; - return; - } - - motor_time_constant_ = k; -} - -const Eigen::Vector3d &Quadrotor::getExternalForce() const -{ - return external_force_; -} -void Quadrotor::setExternalForce(const Eigen::Vector3d &force) -{ - external_force_ = force; -} - -const Eigen::Vector3d &Quadrotor::getExternalMoment() const -{ - return external_moment_; -} -void Quadrotor::setExternalMoment(const Eigen::Vector3d &moment) -{ - external_moment_ = moment; -} - -double Quadrotor::getMaxRPM() const -{ - return max_rpm_; -} -void Quadrotor::setMaxRPM(double max_rpm) -{ - if(max_rpm <= 0) - { - std::cerr << "Max rpm <= 0, not setting" << std::endl; - return; - } - max_rpm_ = max_rpm; -} - -double Quadrotor::getMinRPM() const -{ - return min_rpm_; -} -void Quadrotor::setMinRPM(double min_rpm) -{ - if(min_rpm < 0) - { - std::cerr << "Min rpm < 0, not setting" << std::endl; - return; - } - min_rpm_ = min_rpm; -} - -void Quadrotor::updateInternalState() -{ - for(int i = 0; i < 3; i++) - { - internal_state_[0 + i] = state_.x(i); - internal_state_[3 + i] = state_.v(i); - internal_state_[6 + i] = state_.R(i, 0); - internal_state_[9 + i] = state_.R(i, 1); - internal_state_[12 + i] = state_.R(i, 2); - internal_state_[15 + i] = state_.omega(i); - } - internal_state_[18] = state_.motor_rpm(0); - internal_state_[19] = state_.motor_rpm(1); - internal_state_[20] = state_.motor_rpm(2); - internal_state_[21] = state_.motor_rpm(3); -} - -} // namespace QuadrotorSimulator diff --git a/kr_quadrotor_simulator/src/quadrotor_simulator_base.hpp b/kr_quadrotor_simulator/src/quadrotor_simulator_base.hpp deleted file mode 100644 index 406207e4..00000000 --- a/kr_quadrotor_simulator/src/quadrotor_simulator_base.hpp +++ /dev/null @@ -1,291 +0,0 @@ -#ifndef QUADROTOR_SIMULATOR_BASE_HPP_ -#define QUADROTOR_SIMULATOR_BASE_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace QuadrotorSimulator -{ -template -class QuadrotorSimulatorBase -{ - public: - QuadrotorSimulatorBase(ros::NodeHandle &n); - void run(void); - void extern_force_callback(const geometry_msgs::Vector3Stamped::ConstPtr &f_ext); - void extern_moment_callback(const geometry_msgs::Vector3Stamped::ConstPtr &m_ext); - - protected: - typedef struct _ControlInput - { - double rpm[4]; - } ControlInput; - - /* - * The callback called when ROS message arrives and needs to fill in the - * command_ field that would be passed to the getControl function. - */ - virtual void cmd_callback(const typename T::ConstPtr &cmd) = 0; - - /* - * Called by the simulator to get the current motor speeds. This is the - * controller that would be running on the robot. - * @param[in] quad Quadrotor instance which is being simulated - * @param[in] cmd The command input which is filled by cmd_callback - */ - virtual ControlInput getControl(const Quadrotor &quad, const U &cmd) const = 0; - - Quadrotor quad_; - U command_; - - private: - void stateToOdomMsg(const Quadrotor::State &state, nav_msgs::Odometry &odom) const; - void quadToImuMsg(const Quadrotor &quad, sensor_msgs::Imu &imu) const; - void tfBroadcast(const nav_msgs::Odometry &odom_msg); - - ros::Publisher pub_odom_; - ros::Publisher pub_imu_; - ros::Publisher pub_output_data_; - ros::Subscriber sub_cmd_; - ros::Subscriber sub_extern_force_; - ros::Subscriber sub_extern_moment_; - double simulation_rate_; - double odom_rate_; - std::string quad_name_; - std::string world_frame_id_; - tf2_ros::TransformBroadcaster tf_broadcaster_; -}; - -template -QuadrotorSimulatorBase::QuadrotorSimulatorBase(ros::NodeHandle &n) -{ - pub_odom_ = n.advertise("odom", 100); - pub_imu_ = n.advertise("imu", 100); - pub_output_data_ = n.advertise("output_data", 100); - sub_cmd_ = - n.subscribe("cmd", 100, &QuadrotorSimulatorBase::cmd_callback, this, ros::TransportHints().tcpNoDelay()); - sub_extern_force_ = n.subscribe( - "extern_force", 10, &QuadrotorSimulatorBase::extern_force_callback, this, ros::TransportHints().tcpNoDelay()); - sub_extern_moment_ = n.subscribe( - "extern_moment", 10, &QuadrotorSimulatorBase::extern_moment_callback, this, ros::TransportHints().tcpNoDelay()); - - n.param("rate/simulation", simulation_rate_, 1000.0); - ROS_ASSERT(simulation_rate_ > 0); - - n.param("rate/odom", odom_rate_, 100.0); - - n.param("world_frame_id", world_frame_id_, std::string("simulator")); - n.param("quadrotor_name", quad_name_, std::string("quadrotor")); - - auto get_param = [&n](const std::string ¶m_name) { - double param; - if(!n.hasParam(param_name)) - { - ROS_WARN("Simulator sleeping to wait for param %s", param_name.c_str()); - ros::Duration(0.5).sleep(); - } - if(!n.getParam(param_name, param)) - { - const std::string error_msg = param_name + " not set"; - ROS_FATAL_STREAM(error_msg); - throw std::logic_error(error_msg); - } - return param; - }; - - quad_.setMass(get_param("mass")); - quad_.setInertia(Eigen::Vector3d(get_param("Ixx"), get_param("Iyy"), get_param("Izz")).asDiagonal()); - quad_.setGravity(get_param("gravity")); - quad_.setPropRadius(get_param("prop_radius")); - quad_.setPropellerThrustCoefficient(get_param("thrust_coefficient")); - quad_.setArmLength(get_param("arm_length")); - quad_.setMotorTimeConstant(get_param("motor_time_constant")); - quad_.setMinRPM(get_param("min_rpm")); - quad_.setMaxRPM(get_param("max_rpm")); - quad_.setDragCoefficient(get_param("drag_coefficient")); - - Eigen::Vector3d initial_pos; - n.param("initial_position/x", initial_pos(0), 0.0); - n.param("initial_position/y", initial_pos(1), 0.0); - n.param("initial_position/z", initial_pos(2), 0.0); - - Eigen::Quaterniond initial_q; - n.param("initial_orientation/w", initial_q.w(), 1.0); - n.param("initial_orientation/x", initial_q.x(), 0.0); - n.param("initial_orientation/y", initial_q.y(), 0.0); - n.param("initial_orientation/z", initial_q.z(), 0.0); - initial_q.normalize(); - - Quadrotor::State state = quad_.getState(); - state.x(0) = initial_pos(0); - state.x(1) = initial_pos(1); - state.x(2) = initial_pos(2); - state.R = initial_q.matrix(); - quad_.setState(state); -} - -template -void QuadrotorSimulatorBase::run(void) -{ - // Call once with empty command to initialize values - cmd_callback(boost::make_shared()); - - QuadrotorSimulatorBase::ControlInput control; - - nav_msgs::Odometry odom_msg; - sensor_msgs::Imu imu_msg; - kr_mav_msgs::OutputData output_data_msg; - odom_msg.header.frame_id = world_frame_id_; - odom_msg.child_frame_id = quad_name_; - imu_msg.header.frame_id = quad_name_; - output_data_msg.header.frame_id = quad_name_; - - const double simulation_dt = 1 / simulation_rate_; - ros::Rate r(simulation_rate_); - - const ros::Duration odom_pub_duration(1 / odom_rate_); - ros::Time next_odom_pub_time = ros::Time::now(); - - while(ros::ok()) - { - ros::spinOnce(); - - control = getControl(quad_, command_); - quad_.setInput(control.rpm[0], control.rpm[1], control.rpm[2], control.rpm[3]); - quad_.step(simulation_dt); - - ros::Time tnow = ros::Time::now(); - - if(tnow >= next_odom_pub_time) - { - next_odom_pub_time += odom_pub_duration; - const Quadrotor::State &state = quad_.getState(); - - stateToOdomMsg(state, odom_msg); - odom_msg.header.stamp = tnow; - pub_odom_.publish(odom_msg); - tfBroadcast(odom_msg); - - quadToImuMsg(quad_, imu_msg); - imu_msg.header.stamp = tnow; - pub_imu_.publish(imu_msg); - - // Also publish an OutputData msg - output_data_msg.header.stamp = tnow; - output_data_msg.orientation = imu_msg.orientation; - output_data_msg.angular_velocity = imu_msg.angular_velocity; - output_data_msg.linear_acceleration = imu_msg.linear_acceleration; - output_data_msg.motor_rpm[0] = state.motor_rpm(0); - output_data_msg.motor_rpm[1] = state.motor_rpm(1); - output_data_msg.motor_rpm[2] = state.motor_rpm(2); - output_data_msg.motor_rpm[3] = state.motor_rpm(3); - pub_output_data_.publish(output_data_msg); - } - - r.sleep(); - } -} - -template -void QuadrotorSimulatorBase::extern_force_callback(const geometry_msgs::Vector3Stamped::ConstPtr &f_ext) -{ - quad_.setExternalForce(Eigen::Vector3d(f_ext->vector.x, f_ext->vector.y, f_ext->vector.z)); -} - -template -void QuadrotorSimulatorBase::extern_moment_callback(const geometry_msgs::Vector3Stamped::ConstPtr &m_ext) -{ - quad_.setExternalMoment(Eigen::Vector3d(m_ext->vector.x, m_ext->vector.y, m_ext->vector.z)); -} - -template -void QuadrotorSimulatorBase::stateToOdomMsg(const Quadrotor::State &state, nav_msgs::Odometry &odom) const -{ - odom.pose.pose.position.x = state.x(0); - odom.pose.pose.position.y = state.x(1); - odom.pose.pose.position.z = state.x(2); - - Eigen::Quaterniond q(state.R); - odom.pose.pose.orientation.x = q.x(); - odom.pose.pose.orientation.y = q.y(); - odom.pose.pose.orientation.z = q.z(); - odom.pose.pose.orientation.w = q.w(); - - odom.twist.twist.linear.x = state.v(0); - odom.twist.twist.linear.y = state.v(1); - odom.twist.twist.linear.z = state.v(2); - - odom.twist.twist.angular.x = state.omega(0); - odom.twist.twist.angular.y = state.omega(1); - odom.twist.twist.angular.z = state.omega(2); -} - -template -void QuadrotorSimulatorBase::quadToImuMsg(const Quadrotor &quad, sensor_msgs::Imu &imu) const -{ - const Quadrotor::State state = quad.getState(); - Eigen::Quaterniond q(state.R); - imu.orientation.x = q.x(); - imu.orientation.y = q.y(); - imu.orientation.z = q.z(); - imu.orientation.w = q.w(); - - imu.angular_velocity.x = state.omega(0); - imu.angular_velocity.y = state.omega(1); - imu.angular_velocity.z = state.omega(2); - - const double kf = quad.getPropellerThrustCoefficient(); - const double m = quad.getMass(); - const Eigen::Vector3d &external_force = quad.getExternalForce(); - const double g = quad.getGravity(); - const double thrust = kf * state.motor_rpm.square().sum(); - Eigen::Vector3d acc; - if(state.x(2) < 1e-4) - { - acc = state.R.transpose() * (external_force / m + Eigen::Vector3d(0, 0, g)); - } - else - { - acc = thrust / m * Eigen::Vector3d(0, 0, 1) + state.R.transpose() * external_force / m; - if(quad.getDragCoefficient() != 0) - { - const double drag_coefficient = quad.getDragCoefficient(); - const double mass = quad.getMass(); - Eigen::Matrix3d P; - P << 1, 0, 0, 0, 1, 0, 0, 0, 0; - acc -= drag_coefficient / mass * P * state.R.transpose() * state.v; - } - } - - imu.linear_acceleration.x = acc(0); - imu.linear_acceleration.y = acc(1); - imu.linear_acceleration.z = acc(2); -} - -template -void QuadrotorSimulatorBase::tfBroadcast(const nav_msgs::Odometry &odom_msg) -{ - geometry_msgs::TransformStamped ts; - - ts.header.stamp = odom_msg.header.stamp; - ts.header.frame_id = odom_msg.header.frame_id; - ts.child_frame_id = odom_msg.child_frame_id; - - ts.transform.translation.x = odom_msg.pose.pose.position.x; - ts.transform.translation.y = odom_msg.pose.pose.position.y; - ts.transform.translation.z = odom_msg.pose.pose.position.z; - - ts.transform.rotation = odom_msg.pose.pose.orientation; - - tf_broadcaster_.sendTransform(ts); -} -} // namespace QuadrotorSimulator - -#endif diff --git a/kr_quadrotor_simulator/src/quadrotor_simulator_so3.cpp b/kr_quadrotor_simulator/src/quadrotor_simulator_so3.cpp deleted file mode 100644 index cd9a6875..00000000 --- a/kr_quadrotor_simulator/src/quadrotor_simulator_so3.cpp +++ /dev/null @@ -1,167 +0,0 @@ -#include - -#include - -#include "quadrotor_simulator_base.hpp" - -namespace QuadrotorSimulator -{ -typedef struct _SO3Command -{ - float force[3]; - float qx, qy, qz, qw; - float angular_velocity[3]; - float kR[3]; - float kOm[3]; - float kf_correction; - float angle_corrections[2]; - bool enable_motors; -} SO3Command; - -class QuadrotorSimulatorSO3 : public QuadrotorSimulatorBase -{ - public: - QuadrotorSimulatorSO3(ros::NodeHandle &nh) : QuadrotorSimulatorBase(nh) {} - - private: - virtual void cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &cmd); - virtual ControlInput getControl(const Quadrotor &quad, const SO3Command &cmd) const; -}; - -void QuadrotorSimulatorSO3::cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &cmd) -{ - command_.force[0] = cmd->force.x; - command_.force[1] = cmd->force.y; - command_.force[2] = cmd->force.z; - command_.qx = cmd->orientation.x; - command_.qy = cmd->orientation.y; - command_.qz = cmd->orientation.z; - command_.qw = cmd->orientation.w; - command_.angular_velocity[0] = cmd->angular_velocity.x; - command_.angular_velocity[1] = cmd->angular_velocity.y; - command_.angular_velocity[2] = cmd->angular_velocity.z; - command_.kR[0] = cmd->kR[0]; - command_.kR[1] = cmd->kR[1]; - command_.kR[2] = cmd->kR[2]; - command_.kOm[0] = cmd->kOm[0]; - command_.kOm[1] = cmd->kOm[1]; - command_.kOm[2] = cmd->kOm[2]; - command_.kf_correction = cmd->aux.kf_correction; - command_.angle_corrections[0] = cmd->aux.angle_corrections[0]; // Not used yet - command_.angle_corrections[1] = cmd->aux.angle_corrections[1]; // Not used yet - command_.enable_motors = cmd->aux.enable_motors; -} - -QuadrotorSimulatorSO3::ControlInput QuadrotorSimulatorSO3::getControl(const Quadrotor &quad, - const SO3Command &cmd) const -{ - const double _kf = quad.getPropellerThrustCoefficient(); - const double _km = quad.getPropellerMomentCoefficient(); - const double kf = _kf - cmd.kf_correction; - const double km = _km / _kf * kf; - - const double d = quad.getArmLength(); - const Eigen::Matrix3f J = quad.getInertia().cast(); - const float I[3][3] = {{J(0, 0), J(0, 1), J(0, 2)}, {J(1, 0), J(1, 1), J(1, 2)}, {J(2, 0), J(2, 1), J(2, 2)}}; - const Quadrotor::State &state = quad.getState(); - - float R11 = state.R(0, 0); - float R12 = state.R(0, 1); - float R13 = state.R(0, 2); - float R21 = state.R(1, 0); - float R22 = state.R(1, 1); - float R23 = state.R(1, 2); - float R31 = state.R(2, 0); - float R32 = state.R(2, 1); - float R33 = state.R(2, 2); - - float Om1 = state.omega(0); - float Om2 = state.omega(1); - float Om3 = state.omega(2); - - float Rd11 = cmd.qw * cmd.qw + cmd.qx * cmd.qx - cmd.qy * cmd.qy - cmd.qz * cmd.qz; - float Rd12 = 2 * (cmd.qx * cmd.qy - cmd.qw * cmd.qz); - float Rd13 = 2 * (cmd.qx * cmd.qz + cmd.qw * cmd.qy); - float Rd21 = 2 * (cmd.qx * cmd.qy + cmd.qw * cmd.qz); - float Rd22 = cmd.qw * cmd.qw - cmd.qx * cmd.qx + cmd.qy * cmd.qy - cmd.qz * cmd.qz; - float Rd23 = 2 * (cmd.qy * cmd.qz - cmd.qw * cmd.qx); - float Rd31 = 2 * (cmd.qx * cmd.qz - cmd.qw * cmd.qy); - float Rd32 = 2 * (cmd.qy * cmd.qz + cmd.qw * cmd.qx); - float Rd33 = cmd.qw * cmd.qw - cmd.qx * cmd.qx - cmd.qy * cmd.qy + cmd.qz * cmd.qz; - - float Psi = 0.5f * (3.0f - (Rd11 * R11 + Rd21 * R21 + Rd31 * R31 + Rd12 * R12 + Rd22 * R22 + Rd32 * R32 + Rd13 * R13 + - Rd23 * R23 + Rd33 * R33)); - - if(Psi > 1.0f) // Position control stability guaranteed only when Psi < 1 - ROS_WARN_THROTTLE(1, "Warning Psi = %f > 1", Psi); - - float force = cmd.force[0] * R13 + cmd.force[1] * R23 + cmd.force[2] * R33; - - float eR1 = 0.5f * (R12 * Rd13 - R13 * Rd12 + R22 * Rd23 - R23 * Rd22 + R32 * Rd33 - R33 * Rd32); - float eR2 = 0.5f * (R13 * Rd11 - R11 * Rd13 - R21 * Rd23 + R23 * Rd21 - R31 * Rd33 + R33 * Rd31); - float eR3 = 0.5f * (R11 * Rd12 - R12 * Rd11 + R21 * Rd22 - R22 * Rd21 + R31 * Rd32 - R32 * Rd31); - - float Omd1 = cmd.angular_velocity[0] * (R11 * Rd11 + R21 * Rd21 + R31 * Rd31) + - cmd.angular_velocity[1] * (R11 * Rd12 + R21 * Rd22 + R31 * Rd32) + - cmd.angular_velocity[2] * (R11 * Rd13 + R21 * Rd23 + R31 * Rd33); - float Omd2 = cmd.angular_velocity[0] * (R12 * Rd11 + R22 * Rd21 + R32 * Rd31) + - cmd.angular_velocity[1] * (R12 * Rd12 + R22 * Rd22 + R32 * Rd32) + - cmd.angular_velocity[2] * (R12 * Rd13 + R22 * Rd23 + R32 * Rd33); - float Omd3 = cmd.angular_velocity[0] * (R13 * Rd11 + R23 * Rd21 + R33 * Rd31) + - cmd.angular_velocity[1] * (R13 * Rd12 + R23 * Rd22 + R33 * Rd32) + - cmd.angular_velocity[2] * (R13 * Rd13 + R23 * Rd23 + R33 * Rd33); - - float eOm1 = Om1 - Omd1; - float eOm2 = Om2 - Omd2; - float eOm3 = Om3 - Omd3; - - // TODO: Change this to the new term as in http://arxiv.org/abs/1304.6765: - // Omd^ * J * Omd - float in1 = - Om2 * (I[2][0] * Om1 + I[2][1] * Om2 + I[2][2] * Om3) - Om3 * (I[1][0] * Om1 + I[1][1] * Om2 + I[1][2] * Om3); - float in2 = - Om3 * (I[0][0] * Om1 + I[0][1] * Om2 + I[0][2] * Om3) - Om1 * (I[2][0] * Om1 + I[2][1] * Om2 + I[2][2] * Om3); - float in3 = - Om1 * (I[1][0] * Om1 + I[1][1] * Om2 + I[1][2] * Om3) - Om2 * (I[0][0] * Om1 + I[0][1] * Om2 + I[0][2] * Om3); - - float M1 = -cmd.kR[0] * eR1 - cmd.kOm[0] * eOm1 + in1; - float M2 = -cmd.kR[1] * eR2 - cmd.kOm[1] * eOm2 + in2; - float M3 = -cmd.kR[2] * eR3 - cmd.kOm[2] * eOm3 + in3; - - float w_sq[4]; - w_sq[0] = force / (4 * kf) - M2 / (2 * d * kf) + M3 / (4 * km); - w_sq[1] = force / (4 * kf) + M2 / (2 * d * kf) + M3 / (4 * km); - w_sq[2] = force / (4 * kf) + M1 / (2 * d * kf) - M3 / (4 * km); - w_sq[3] = force / (4 * kf) - M1 / (2 * d * kf) - M3 / (4 * km); - - ControlInput control; - for(int i = 0; i < 4; i++) - { - if(cmd.enable_motors) - { - if(w_sq[i] < 0) - w_sq[i] = 0; - - control.rpm[i] = sqrtf(w_sq[i]); - } - else - { - control.rpm[i] = 0; - } - } - return control; -} -} // namespace QuadrotorSimulator - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "kr_quadrotor_simulator_so3"); - - ros::NodeHandle nh("~"); - - QuadrotorSimulator::QuadrotorSimulatorSO3 quad_sim(nh); - - quad_sim.run(); - - return 0; -} diff --git a/kr_quadrotor_simulator/src/quadrotor_simulator_trpy.cpp b/kr_quadrotor_simulator/src/quadrotor_simulator_trpy.cpp deleted file mode 100644 index 05e2f61f..00000000 --- a/kr_quadrotor_simulator/src/quadrotor_simulator_trpy.cpp +++ /dev/null @@ -1,164 +0,0 @@ -#include - -#include - -#include "quadrotor_simulator_base.hpp" - -namespace QuadrotorSimulator -{ -typedef struct _TRPYCommand -{ - float thrust; - float roll, pitch, yaw; - float angular_velocity[3]; - float kR[3]; - float kOm[3]; - bool enable_motors; -} TRPYCommand; - -class QuadrotorSimulatorTRPY : public QuadrotorSimulatorBase -{ - public: - QuadrotorSimulatorTRPY(ros::NodeHandle &nh) : QuadrotorSimulatorBase(nh) {} - - private: - virtual void cmd_callback(const kr_mav_msgs::TRPYCommand::ConstPtr &cmd); - virtual ControlInput getControl(const Quadrotor &quad, const TRPYCommand &cmd) const; -}; -void QuadrotorSimulatorTRPY::cmd_callback(const kr_mav_msgs::TRPYCommand::ConstPtr &cmd) -{ - command_.thrust = cmd->thrust; - command_.roll = cmd->roll; - command_.pitch = cmd->pitch; - command_.yaw = cmd->yaw; - command_.angular_velocity[0] = cmd->angular_velocity.x; - command_.angular_velocity[1] = cmd->angular_velocity.y; - command_.angular_velocity[2] = cmd->angular_velocity.z; - command_.kR[0] = cmd->kR[0]; - command_.kR[1] = cmd->kR[1]; - command_.kR[2] = cmd->kR[2]; - command_.kOm[0] = cmd->kOm[0]; - command_.kOm[1] = cmd->kOm[1]; - command_.kOm[2] = cmd->kOm[2]; - command_.enable_motors = cmd->aux.enable_motors; -} - -QuadrotorSimulatorTRPY::ControlInput QuadrotorSimulatorTRPY::getControl(const Quadrotor &quad, - const TRPYCommand &cmd) const -{ - const double _kf = quad.getPropellerThrustCoefficient(); - const double _km = quad.getPropellerMomentCoefficient(); - const double kf = _kf; - const double km = _km / _kf * kf; - - const double d = quad.getArmLength(); - const Eigen::Matrix3f J = quad.getInertia().cast(); - const float I[3][3] = {{J(0, 0), J(0, 1), J(0, 2)}, {J(1, 0), J(1, 1), J(1, 2)}, {J(2, 0), J(2, 1), J(2, 2)}}; - const Quadrotor::State &state = quad.getState(); - - float R11 = state.R(0, 0); - float R12 = state.R(0, 1); - float R13 = state.R(0, 2); - float R21 = state.R(1, 0); - float R22 = state.R(1, 1); - float R23 = state.R(1, 2); - float R31 = state.R(2, 0); - float R32 = state.R(2, 1); - float R33 = state.R(2, 2); - - float Om1 = state.omega(0); - float Om2 = state.omega(1); - float Om3 = state.omega(2); - - float Rd11 = cos(cmd.yaw) * cos(cmd.pitch); - float Rd12 = cos(cmd.yaw) * sin(cmd.pitch) * sin(cmd.roll) - cos(cmd.roll) * sin(cmd.yaw); - float Rd13 = sin(cmd.yaw) * sin(cmd.roll) + cos(cmd.yaw) * cos(cmd.roll) * sin(cmd.pitch); - float Rd21 = cos(cmd.pitch) * sin(cmd.yaw); - float Rd22 = cos(cmd.yaw) * cos(cmd.roll) + sin(cmd.yaw) * sin(cmd.pitch) * sin(cmd.roll); - float Rd23 = cos(cmd.roll) * sin(cmd.yaw) * sin(cmd.pitch) - cos(cmd.yaw) * sin(cmd.roll); - float Rd31 = -sin(cmd.pitch); - float Rd32 = cos(cmd.pitch) * sin(cmd.roll); - float Rd33 = cos(cmd.pitch) * cos(cmd.roll); - - float Psi = 0.5f * (3.0f - (Rd11 * R11 + Rd21 * R21 + Rd31 * R31 + Rd12 * R12 + Rd22 * R22 + Rd32 * R32 + Rd13 * R13 + - Rd23 * R23 + Rd33 * R33)); - - float force = 0; - if(Psi < 1.0f) // Position control stability guaranteed only when Psi < 1 - force = cmd.thrust; - - float eR1 = 0.5f * (R12 * Rd13 - R13 * Rd12 + R22 * Rd23 - R23 * Rd22 + R32 * Rd33 - R33 * Rd32); - float eR2 = 0.5f * (R13 * Rd11 - R11 * Rd13 - R21 * Rd23 + R23 * Rd21 - R31 * Rd33 + R33 * Rd31); - float eR3 = 0.5f * (R11 * Rd12 - R12 * Rd11 + R21 * Rd22 - R22 * Rd21 + R31 * Rd32 - R32 * Rd31); - - float Omd1 = cmd.angular_velocity[0] * (R11 * Rd11 + R21 * Rd21 + R31 * Rd31) + - cmd.angular_velocity[1] * (R11 * Rd12 + R21 * Rd22 + R31 * Rd32) + - cmd.angular_velocity[2] * (R11 * Rd13 + R21 * Rd23 + R31 * Rd33); - float Omd2 = cmd.angular_velocity[0] * (R12 * Rd11 + R22 * Rd21 + R32 * Rd31) + - cmd.angular_velocity[1] * (R12 * Rd12 + R22 * Rd22 + R32 * Rd32) + - cmd.angular_velocity[2] * (R12 * Rd13 + R22 * Rd23 + R32 * Rd33); - float Omd3 = cmd.angular_velocity[0] * (R13 * Rd11 + R23 * Rd21 + R33 * Rd31) + - cmd.angular_velocity[1] * (R13 * Rd12 + R23 * Rd22 + R33 * Rd32) + - cmd.angular_velocity[2] * (R13 * Rd13 + R23 * Rd23 + R33 * Rd33); - - float eOm1 = Om1 - Omd1; - float eOm2 = Om2 - Omd2; - float eOm3 = Om3 - Omd3; - -#if 0 - float in1 = Om2 * (I[2][0] * Om1 + I[2][1] * Om2 + I[2][2] * Om3) - - Om3 * (I[1][0] * Om1 + I[1][1] * Om2 + I[1][2] * Om3); - float in2 = Om3 * (I[0][0] * Om1 + I[0][1] * Om2 + I[0][2] * Om3) - - Om1 * (I[2][0] * Om1 + I[2][1] * Om2 + I[2][2] * Om3); - float in3 = Om1 * (I[1][0] * Om1 + I[1][1] * Om2 + I[1][2] * Om3) - - Om2 * (I[0][0] * Om1 + I[0][1] * Om2 + I[0][2] * Om3); -#else - float in1 = Omd2 * (I[2][0] * Omd1 + I[2][1] * Omd2 + I[2][2] * Omd3) - - Omd3 * (I[1][0] * Omd1 + I[1][1] * Omd2 + I[1][2] * Omd3); - float in2 = Omd3 * (I[0][0] * Omd1 + I[0][1] * Omd2 + I[0][2] * Omd3) - - Omd1 * (I[2][0] * Omd1 + I[2][1] * Omd2 + I[2][2] * Omd3); - float in3 = Omd1 * (I[1][0] * Omd1 + I[1][1] * Omd2 + I[1][2] * Omd3) - - Omd2 * (I[0][0] * Omd1 + I[0][1] * Omd2 + I[0][2] * Omd3); -#endif - - float M1 = -cmd.kR[0] * eR1 - cmd.kOm[0] * eOm1 + in1; - float M2 = -cmd.kR[1] * eR2 - cmd.kOm[1] * eOm2 + in2; - float M3 = -cmd.kR[2] * eR3 - cmd.kOm[2] * eOm3 + in3; - - float w_sq[4]; - w_sq[0] = force / (4 * kf) - M2 / (2 * d * kf) + M3 / (4 * km); - w_sq[1] = force / (4 * kf) + M2 / (2 * d * kf) + M3 / (4 * km); - w_sq[2] = force / (4 * kf) + M1 / (2 * d * kf) - M3 / (4 * km); - w_sq[3] = force / (4 * kf) - M1 / (2 * d * kf) - M3 / (4 * km); - - ControlInput control; - for(int i = 0; i < 4; i++) - { - if(cmd.enable_motors) - { - if(w_sq[i] < 0) - w_sq[i] = 0; - - control.rpm[i] = sqrtf(w_sq[i]); - } - else - { - control.rpm[i] = 0; - } - } - return control; -} -} // namespace QuadrotorSimulator - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "kr_quadrotor_simulator_trpy"); - - ros::NodeHandle n("~"); - - QuadrotorSimulator::QuadrotorSimulatorTRPY quad_sim(n); - - quad_sim.run(); - - return 0; -} From 637ff20bc5e12cbfd2081eecac55d2a582836766 Mon Sep 17 00:00:00 2001 From: Dexter Date: Thu, 5 Mar 2026 22:10:06 +0000 Subject: [PATCH 64/64] Updated robot namespace --- kr_mav_manager/src/manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 7a07dc04..145afc66 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -53,7 +53,7 @@ MAVManager::MAVManager() this->declare_parameter("mass", rclcpp::PARAMETER_DOUBLE); this->declare_parameter("odom_timeout", 0.1f); this->declare_parameter("takeoff_height", 0.5); - this->declare_parameter("robot", "neurofly1"); + this->declare_parameter("robot", "quadrotor"); std::string robot_name = this->get_parameter("robot").as_string(); RCLCPP_INFO(this->get_logger(), robot_name.c_str());